Animal husbandry pushing robot autonomous navigation method based on laser radar

Abstract

The invention relates to an agricultural robot, and discloses an animal husbandry pushing robot autonomous navigation method based on a laser radar, which comprises a microsecond-level multi-sensor time synchronization architecture based on an FPGA (Field Programmable Gate Array); in a navigation layer, seamless switching between the GNSS and the laser SLAM is realized by constructing a virtual fusion observation value by utilizing extended Kalman filtering and a self-adaptive dynamic weight algorithm; in the sensing layer, an RGB-I multi-mode model fused with laser reflectivity and a biological micro-motion detection technology are adopted, and feed and biological obstacles are accurately distinguished; and in the execution layer, flexible self-adaptive material pushing is achieved in combination with material groove edge fitting and force-position hybrid control, and the navigation precision, the operation quality and the safety of the material pushing robot in the all-weather and unstructured pasture environment are effectively improved.

Description

Technical Field

[0001] This invention relates to agricultural robots, specifically to an autonomous navigation method for a livestock feeding robot based on lidar. Background Technology

[0002] With the increasing intensification of livestock farming, TMR (Total Mixed Ration) feeding technology has become widespread. During feeding, dairy cows push feed out of the feeding area, requiring feed pushing operations to reposition the feed. Currently, feed pushing operations mainly rely on manually driven diesel vehicles or early, simple automated equipment. However, in practical applications, existing navigation and control technologies face specific technical bottlenecks, making it difficult to meet the demands of all-weather, unmanned operations. Firstly, regarding the navigation problem in the unique "indoor-outdoor mixed environment" of ranches, existing technologies typically employ a single navigation mode. For example, outdoor robots using GNSS (Global Navigation Satellite System) cannot locate themselves inside a fully enclosed metal-roofed cattle shed. AGVs relying solely on 2D LiDAR are prone to positioning loss when entering open outdoor passageways or encountering rain or snow, due to sparse environmental geometry or drastic changes in road texture. Existing technologies often use hard-switching logic (i.e., directly switching sensor sources at a certain point) when handling indoor-outdoor transition areas. This approach ignores the jumps and delays in sensor data during switching, easily causing the robot to experience positional oscillations, sudden stops, or even collisions with fences at the cattle shed entrance, making smooth, continuous operation impossible. Secondly, in terms of object recognition and obstacle avoidance, existing technologies lack the ability to understand environmental semantics. Traditional obstacle avoidance systems are mostly based on infrared or ultrasonic ranging principles, treating all protrusions in front as obstacles. However, the working path of a pushing robot naturally contains "feed piles" (the target to be pushed) and "obstacles" (such as lying cows, workers, and tools). Existing technologies cannot distinguish between the two, causing the robot to frequently misjudge the feed to be pushed as an obstacle and stop frequently, or to pose a collision risk to stationary cows when biological characteristics cannot be distinguished. This problem is even more prominent at night when there is insufficient light and vision is impaired. Furthermore, in terms of feed pushing execution control, existing equipment mostly adopts fixed trajectory tracking and rigid control. The feed trough retaining walls in pastures are not ideally straight lines and often have construction errors, breaks, or bends. Robots traveling along fixed trajectories cannot adaptively conform to the edges of the feed troughs, resulting in incomplete feed pushing or hard friction between the pusher plate and the wall. In addition, in the harsh winter environment, frozen ice blocks are often mixed in with the feed piles. The existing position control mode lacks a force feedback mechanism, and the pusher plate still forcibly extends when encountering hard obstacles, which can easily lead to motor overload and burnout or mechanical damage. Finally, the pasture road environment is complex, including slippery cement, muddy dirt roads and snow-covered roads. The motion planning of traditional wheeled robots does not take into account the dynamic changes in the road friction coefficient. When performing turns or acceleration and deceleration, they are prone to slipping and losing control due to insufficient adhesion, which in turn leads to the divergence of odometer errors and affects navigation accuracy. Summary of the Invention

[0003] The purpose of this invention is to provide an autonomous navigation method for livestock feeding robots based on lidar, in order to solve the problems mentioned in the background art. To achieve the above objectives, this invention provides an autonomous navigation method for livestock feeding robots based on lidar, which is applied to unmanned feeding robots. The robot is equipped with a 32-line mechanical lidar, a binocular camera, an inertial measurement unit, a global navigation satellite system receiver, and an automotive-grade drive-by-wire chassis. The method includes: Step 1: Construct a hard-triggered synchronization mechanism based on field-programmable gate arrays, align all sensor data to the second pulse time axis of the global navigation satellite system, and establish a hard-coupled hybrid map model that includes geographic coordinate system, local odometry coordinate system and laser map coordinate system. The real-time latitude and longitude are projected into the laser map coordinate system through the static transformation matrix calculated by offline mapping. Step 2: Construct an extended Kalman filter model based on error state. When the robot is in the outdoor-to-indoor transition buffer zone where satellite signals are limited, it does not directly switch the observation source. Instead, it calculates the ratio of the observation covariance of the global navigation satellite system to the laser positioning matching degree in real time, and generates dynamic weight coefficients accordingly. It then constructs a virtual fusion observation value containing satellite positioning points and laser matching points and inputs it into the filter model. Step 3: Based on the obtained fused pose, the reflectivity intensity and geometric distribution characteristics of the lidar point cloud are used to simultaneously identify the edge curve of the trough and the biological signs in front. Based on this, the pusher's edge extension and retraction commands and the chassis' obstacle avoidance movement commands are output. Furthermore, in the adaptive fusion positioning step, the generation logic of the dynamic weight coefficients is as follows: A distance attenuation function is established that varies with the robot's distance into the buffer zone in an S-shaped curve; the first variance corresponding to the horizontal accuracy factor of the satellite signal and the second variance corresponding to the trace of the Hessian matrix inverse of the laser point cloud normal distribution transformation matching algorithm are calculated in real time; the distance attenuation function is multiplied by the signal confidence factor to obtain the final weight coefficients, wherein the signal confidence factor is negatively correlated with the first variance and positively correlated with the second variance. Furthermore, in the spatiotemporal reference unification step, the hard-triggered synchronization mechanism is specifically configured as follows: using a field-programmable gate array (FPGA) as the core controller, employing precise time protocol network messages to synchronize the lidar, using physical level pulses to trigger the exposure of the binocular camera, and using external interrupts to respond to inertial measurement unit data, thereby limiting the time synchronization error of the multi-source sensors to the microsecond range. Furthermore, the method also includes an online automatic extrinsic parameter calibration process: With the robot in a non-degenerate motion excitation state, a nonlinear least-squares optimization objective function is constructed. This objective function aims to minimize the residual between the relative pose trajectory calculated by the laser odometry and the relative pose trajectory calculated by the visual-inertial odometry in the rigid body transformation closed loop, thereby iteratively solving for the static transformation matrix of the camera relative to the lidar. Furthermore, in the operation and motion control steps, the following strategy is adopted for the identification of the trough edge and the control of the pusher plate: In the extracted point cloud of the region of interest, outliers belonging to the noise are removed using a random sampling consensus algorithm to obtain a cleaned set of inner points on the edge; the least squares method is used to perform linear fitting on the inner point set to obtain the heading deviation and lateral distance deviation; based on the deviation, the extension and retraction speed of the pusher plate is adjusted through a proportional-derivative controller. Furthermore, the pusher control also incorporates a force-position hybrid control logic based on an admittance model: real-time monitoring of the load feedback value of the pusher drive mechanism; when the load feedback value exceeds a set safety threshold, a reverse position correction amount proportional to the load excess value is generated and superimposed on the theoretical extension position command of the pusher, enabling the pusher to exhibit flexible retraction characteristics. Furthermore, the identification of biological signs in front employs a dual-verification logic: the first verification involves projecting the reflectivity intensity of the laser point cloud onto a visual image to generate a four-channel tensor, which is then used by a deep learning model to distinguish between biological and non-biological targets; the second verification involves, for suspected stationary biological targets, calculating the radial distance variance of their point cloud centroid within a preset time window. When this variance exceeds a preset respiratory micro-movement threshold, the target is determined to possess biological activity. Furthermore, the obstacle avoidance motion command generation of the chassis includes active anti-skid planning based on friction circle constraints: a dynamic friction coefficient layer is established in the environmental map, and dynamic constraints are introduced during path planning to limit the maximum planned speed at path points, ensuring that the resultant force of the longitudinal traction and lateral centripetal force required by the vehicle at that path point does not exceed the maximum adhesion force that the current road surface friction coefficient can provide. Furthermore, the chassis motion control adopts a four-wheel independent drive torque vector distribution strategy: the slip ratio of each wheel is calculated in real time, and when the slip ratio exceeds the optimal adhesion range, the slip mode controller reduces the torque of the slipping wheel and simultaneously increases the torque of the non-slipping wheel to generate a yaw moment to correct the vehicle's attitude. Furthermore, the method also includes a variable frequency oscillation traction control mode: when the vehicle is detected to be in trouble and the wheels are spinning, the drive motor is controlled to output a sinusoidal torque that includes a basic bias and oscillation amplitude, and the frequency and phase of the sinusoidal torque are dynamically adjusted in combination with the vehicle pitch angle phase fed back by the inertial measurement unit, so as to increase the instantaneous grip of the tires by utilizing the resonance effect.

[0004] Compared with the prior art, the beneficial effects of the present invention are: 1. Achieved smooth and seamless navigation in all indoor and outdoor scenarios: By constructing a hard-coupled model of geographic coordinate system and laser map coordinate system, and adopting the adaptive dynamic weight EKF algorithm based on signal confidence, this invention solves the transition problem between GNSS signal-limited areas and laser feature degradation areas for robots. This "soft switching" mechanism eliminates positioning jumps and ensures that the robot can continuously and stably shuttle between the inside and outside of the cowshed without human intervention. 2. Improved operation quality and equipment safety in unstructured environments: This invention integrates RANSAC feature extraction and force-position hybrid control strategy. The robot can not only adaptively fit the irregular edge curve of the trough to ensure thorough material pushing, but also sense load changes through current / pressure feedback. When encountering frozen ice or abnormal resistance, the push plate exhibits flexible yielding characteristics, effectively avoiding motor overload and mechanical damage, and adapting to all-weather pasture environments. 3. Solved the semantic conflict and nighttime operation problems in complex scenarios: By using the RGB-I multimodal recognition model with enhanced laser reflectivity intensity, combined with the dual verification mechanism of biological signs (micro-breathing movements), the robot can accurately distinguish between "feed piles" and "biological obstacles" in complete darkness or under drastic changes in light. This completely solves the contradiction of traditional equipment "stopping when it sees grass" or "accidentally injuring lying cows", and achieves intelligent obstacle avoidance with zero misjudgment. 4. Enhanced driving stability under extreme road conditions: By introducing active anti-skid planning based on road friction circle constraints and torque vector distribution of four-wheel independent drive, the robot can actively limit the planned speed and correct the vehicle posture when driving on snowy or muddy roads, effectively preventing slippage and loss of control, and ensuring attendance rate in bad weather. Attached Figure Description

[0005] Figure 1 Schematic diagram of hardware-level time synchronization for multiple sensors; Figure 2 A logic diagram for smooth switching between indoor and outdoor navigation; Figure 3 This is a force-position hybrid adaptive feeding control diagram; Figure 4 This is a multimodal semantic perception and verification graph; Figure 5 This is a schematic diagram of the active anti-slip and obstacle-avoidance control principle. Detailed Implementation

[0006] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention. Please see Figures 1-5 This invention provides an autonomous navigation method for a livestock feeding robot based on lidar; Example 1 like Figure 1 As shown in the figure, this embodiment elaborates on a hardware system architecture for an unmanned feeding robot applied to complex environments in animal husbandry. This architecture not only solves the problem of perception failure of a single sensor in environments with changes in lighting, high dust, and lack of texture features, but also solves the problem of perception data alignment when the robot is operating at a high speed of 15km / h through a microsecond-level time synchronization mechanism and an online extrinsic parameter calibration model, thus ensuring the accuracy of navigation and obstacle avoidance.

[0007] The hardware system of this embodiment mainly consists of three parts: a perception subsystem, a computing and control subsystem, and an execution subsystem. The physical connections and layout of each part have been specifically designed to adapt to the pasture environment. First, in the core configuration of the perception subsystem, a 32-line mechanical rotating lidar is installed at the center of the top of the robot, with the installation height set at 1.8 meters above the ground. The reason for choosing a 32-line rather than a low-beam lidar is that the edge height of the feed trough in a pasture is usually 30cm-50cm, and the cattle pen fence is made of thin tubes. Low-beam lidar is prone to missed detection at close range. This lidar has a vertical field of view of -15° to +15° and a horizontal field of view of 360°, which can... The frequency generates high-density point clouds, effectively covering the shape of the material pile and the characteristics of the cattle shed pillars within a 50-meter range in front. In order to eliminate blind spots at close range, 10 sets of ultrasonic radars are embedded around the vehicle body (front bumper, rear bumper and side skirts).

[0008] The four front cameras are primarily used to detect foreign objects (such as stones) or lying cattle at the bottom of the feed pile; the two side cameras are used to monitor the distance to the fence when making a U-turn in a narrow passage within the cattle shed; and the two rear cameras are used for collision avoidance when reversing. Additionally, a binocular intelligent camera system is integrated at the front of the vehicle roof. Positioning antennas and smart cameras are used to capture color and texture information to help determine the semantic attributes of obstacles; The antenna is used for centimeter-level positioning in unobstructed outdoor areas. A six-axis high-precision inertial measurement unit (IMU) is rigidly connected to the center of the vehicle body to measure the vehicle's three-axis acceleration and three-axis angular velocity, which serve as the front-end calculation basis for SLAM positioning.

[0009] In terms of the computing and control subsystem, an automotive-grade domain controller is used as the "intelligent brain". Considering the extremely high computing power requirements of multi-sensor fusion, the computing platform adopts an embedded GPU architecture (such as NVIDIA Jetson AGX Orin) with an AI computing power of more than 200 TOPS to run the YOLOv5 semantic recognition model and Lio-SAM laser positioning algorithm. The underlying control adopts an automotive-grade drive-by-wire chassis and communicates with the computing platform through the CAN bus to achieve millisecond-level response control of the steering, drive, braking and push plate hydraulic systems, ensuring the power distribution of the robot in complex working conditions such as "steep slopes" and "slippery surfaces".

[0010] In a microsecond-level hard-triggered time synchronization mechanism, if there is a 10ms time deviation between the sensors during material pushing operations at a speed of 15km / h (approximately 4.17m / s), it will result in a spatial position error of more than 4cm, which is unacceptable in edge-pushing operations. Therefore, this embodiment designs a hardware-level time synchronization system based on FPGA. This system uses the PPS (Pulse Per Second) signal output by the GNSS receiver as the global master clock source. The GNSS receiver outputs a high-precision pulse per second with a rise edge accuracy better than 20ns, and simultaneously outputs the NMEA containing the UTC time corresponding to the pulse through the serial port. The FPGA acts as a synchronization controller, receiving PPS signals and NMEA messages, and maintaining a local high-precision microsecond counter. For the 32-line LiDAR, the PTP (Precision Time Protocol, IEEE 1588v2) protocol is used for synchronization. The LiDAR acts as a PTP Slave (slave clock), and the computing platform acts as a PTP Master (master clock). After the FPGA parses the PPS signal and calibrates the system time, it sends a synchronization message to the LiDAR via Ethernet. The specific process follows the PTP protocol's delay response mechanism: the Master sends a Sync message, and the Slave records the reception time. The Master sends a Follow_Up message containing the sending time. The slave sends a Delay_Req record to track the sending time. The Master's response, Delay_Resp, includes the received time. Transmission delay and time deviation The calculation formula is as follows: Through this mechanism, the synchronization accuracy between the lidar's internal timestamp and the system time can be controlled within 1 microsecond. For the smart camera and IMU, a hard trigger mode is adopted. The FPGA sends TTL level pulses to the camera's external trigger pin at a frequency of 10Hz or 20Hz. The camera starts exposure the instant it receives the rising edge of the pulse and records the timestamp of the exposure moment. This timestamp directly corresponds to the FPGA's local time, thereby eliminating the uncertainty delay caused by the operating system scheduling. The IMU is configured in external interrupt mode. It sends an interrupt signal to the FPGA every time a set of data is generated, and the FPGA immediately timestamps it. Through the above mechanism, the system achieves strict alignment of all sensor data on the time axis, with a maximum time synchronization error of no more than 10 microseconds, and the displacement error at a speed of 15km / h is negligible.

[0011] Construction and Application of Online Extrinsic Parameter Calibration Model under Complex Vibration Environment Ranch roads are mostly unpaved, with "snow accumulation", "frozen soil" and "potholes". The robot is in a high-frequency vibration state for a long time, which may cause slight deviations in the sensor extrinsic parameters (installation position and angle). In order to ensure the perception accuracy, this embodiment constructs an online extrinsic parameter automatic calibration model based on motion constraints.

[0012] The model is based on a variant of the "hand-eye calibration" theory, which is the equivalence of the motion trajectory calculated by lidar and the motion trajectory calculated by vision / IMU in terms of rigid body transformation.

[0013] Let the coordinate system of the lidar be L, the coordinate system of the camera be C, and the coordinate system of the base (vehicle body) be [missing coordinate system]. What we need to solve is the static transformation matrix from the camera to the lidar. Defined in time period arrive Inside, the relative motion calculated by the lidar is The relative motion calculated by the camera using visual odometry is , Based on rigid body kinematic constraints, the following closed-loop relationship exists: The above equation is the classic one. However, in actual online calibration, we transform it into a nonlinear least squares optimization problem.

[0014] Optimize the objective function and construct the error function based on its physical meaning. The goal is to minimize the residuals of rotation and translation: Explanation of physical meaning: The unknown variable to be optimized is the rotation matrix of the camera relative to the LiDAR. Translation vector Lie algebras are typically used. Parameterization is performed to avoid the orthogonality constraint problem of the rotation matrix. The pose change calculated by the laser SLAM algorithm between two consecutive frames represents "how the car moves (from the radar's perspective)". The pose change calculated by the visual algorithm between two consecutive frames represents "how the car moves (from the camera's perspective)". Li Qun Mapping to Lie algebra In essence, it converts the rotation matrix into a rotation vector, which facilitates the calculation of the Euclidean distance error. Mahalanobis distance, introducing the covariance matrix The observation noise is weighted, with the weight of data from periods of intense vibration decreasing. Use Cases and Processes: This model is embedded in the robot's background computing process. When the system detects that the robot is traveling in a straight line and then making a slow turn (with sufficient excitation) and the road surface features are rich (such as the texture of the pillars inside the cowshed), the system automatically triggers the "online calibration" thread. Data filtering: Select vehicle speed and angular velocity Data segments should be used to avoid degenerate motion (such as pure linear motion where some rotational components cannot be calibrated).

[0015] Trajectory synchronization: Utilizing the aforementioned microsecond-level time synchronization, the laser frame and visual frame are strictly aligned. Iterative solution: The Levenberg-Marquardt (LM) algorithm is used to iteratively solve the above objective function and update it. parameter.

[0016] Result verification: When the variance of multiple consecutive calculations is less than a threshold (e.g., rotation error) Translation error When updating the system parameter file, the robot can automatically compensate for sensor mechanical deformation caused by pasture bumps without relying on a specific calibration plate, ensuring long-term operational stability.

[0017] To address the high ammonia corrosion, high humidity, and dust environment of the ranch, all exposed sensor interfaces use aviation connectors with an IP67 protection rating. A steel wire rope shock absorber is designed below the lidar to filter out the 10-50Hz low-frequency large-amplitude vibrations commonly found on ranch roads, preventing damage to the internal bearings of the lidar. The computing unit adopts a fanless passive cooling design, placed in a sealed aluminum alloy chassis, and conducts heat to the vehicle's metal frame through heat pipes, avoiding the fan from sucking in cow hair and dust, which could cause overheating and stalling.

[0018] As an alternative architecture to this embodiment, considering the needs of cost-sensitive ranches, a configuration strategy of "16-line LiDAR + solid-state blind spot radar" can be adopted. In this architecture, the main sensor is replaced with a 16-line LiDAR. Although the vertical resolution is reduced, the 360° horizontal mapping capability is retained. In order to compensate for the 16-line radar's insufficient ability to detect near-field ground obstacles (such as low rocks and manure piles), a wide-angle solid-state LiDAR is added to each side of the front of the vehicle.

[0019] Architecture differences: The main radar is only responsible for SLAM localization and long-range large object detection; the solid-state radar is responsible for high-resolution 3D obstacle avoidance at close range.

[0020] Data fusion adjustment: In the data processing layer, the point cloud of the solid-state radar needs to be stitched into the coordinate system of the main radar through extrinsic parameter transformation. Since solid-state radar is usually non-repeating or has a fixed field of view, its data update model needs to add an independent observation channel in the above Kalman filtering.

[0021] Advantages: This alternative reduces sensor hardware costs by about 30% while achieving detection effects similar to or even better than 32-line radar in near-ground areas through multi-radar field-of-view stitching. However, it places higher demands on the performance of the processor's multi-path point cloud stitching algorithm.

[0022] In summary, this embodiment, by constructing a high-specification hardware physical architecture and combining a rigorous time synchronization mechanism and an intelligent online calibration model, forms a complete and practical basic platform for the perception and control of the pushing robot, effectively supporting its goal of unmanned operation in all weather and multiple scenarios.

[0023] Example 2 like Figure 2 As shown, this embodiment discloses a hybrid navigation and positioning system and method for a pusher robot. The method solves the problem of positioning jumps and getting lost caused by satellite signal loss or environmental feature degradation in the mixed scenario of "outdoor open road-semi-enclosed transition zone-fully enclosed cowshed" unique to ranches by constructing a multi-level state estimation model and an adaptive weight fusion algorithm. This embodiment not only describes the smooth switching logic under normal working conditions, but also discloses in detail the fault diagnosis and degraded operation strategy under extreme working conditions.

[0024] To achieve seamless switching between indoor and outdoor environments, a unified mathematical benchmark must first be established. In this embodiment, a hard coupling alignment strategy is adopted between the "geographic coordinate system", the "local odometer coordinate system" and the "map coordinate system".

[0025] Before using the robot for operations, map model building requires preprocessing. The robot is remotely controlled to move from outdoors to indoors, with a 32-line LiDAR, IMU, and RTK-GNSS activated throughout the process. A tightly coupled LiDAR-inertial odometry and mapping algorithm (such as LIO-SAM) are used to construct an environmental point cloud map. During the mapping process, GNSS factors are introduced as global constraints to build a factor graph optimization model with the objective function... Defined as minimizing the sum of Mahalanobis distances for all factors: Explanation of physical meaning: The state variables of the robot's entire trajectory to be optimized; , , These represent the IMU pre-integration residual, the laser odometry residual, and the GNSS position residual, respectively. , , These are the corresponding covariance matrices. Through the above optimization, the generated laser point cloud map itself contains absolute geographic coordinate information. The system calculates and saves a static transformation matrix. It is used to project real-time latitude and longitude coordinates into the laser map coordinate system, thereby ensuring the natural overlap of indoor and outdoor positioning data in geometric space, which is a prerequisite for achieving seamless switching.

[0026] In the real-time operation of the material-pushing robot, the core positioning engine of the adaptive extended Kalman filter-based real-time fusion positioning model integrates wheel speed odometer, IMU, laser SLAM pose, and GNSS position data. State vectors and equations of motion define the state vectors of a system. for: ,in For three-dimensional position, For speed, For attitude quaternions, The zero biases of the accelerometer and gyroscope are used respectively. The prediction stage mainly relies on high-frequency IMU data (200Hz) and uses a mechanical orchestration algorithm to predict the state at the next moment. Covariance Matrix : Physical meaning: This represents the uncertainty of the system's estimation of the current position; It is the state transition Jacobian matrix; It is the process noise covariance of the IMU. This step ensures that even if all external observations (GPS / laser) are lost, the robot can still maintain accurate positioning for a short time (<5 seconds) by inertia.

[0027] When the robot is in the "buffer zone" (usually defined as the area 5 meters outside to 5 meters inside the cowshed door), the system needs to handle the dynamic process of the GNSS signal gradually diverging and the laser feature gradually being established. This embodiment designs a dynamic weight fusion observation equation based on variance adaptive.

[0028] The standard observation update formula is: The innovation of this embodiment lies in that it does not directly switch the observation source, but instead constructs a virtual fusion observation location. Input to EKF Among them, adaptive weight coefficients The calculation model no longer relies solely on distance, but introduces a "signal confidence" factor, as shown in the following formula: Detailed explanation of the physical meaning: and : RTK positioning points and laser matching positioning points after being mapped to coordinate system one, respectively Distance decay function, with the cowshed entrance as the origin. Outdoors is positive, when hour, ;when hour, The middle region transitions smoothly with a Sigmoid curve. The trace of GNSS observation covariance is calculated in real time by the HDOP (Horizontal Precision Factor) of the RTK receiver and the number of satellites. The worse the signal, the larger this value.

[0029] The observation covariance of laser SLAM is derived from the trace of the inverse Hessian matrix of the NDT (Normal Transform) matching algorithm, or simplified by using the reciprocal of the matching score. The fewer features (e.g., an empty doorway), the larger this value.

[0030] Dynamic adjustment mechanism: This formula achieves "survival of the fittest," even when the robot is outdoors ( If occlusion causes RTK variance A dramatic increase, coefficient The system will automatically reduce its reliance on laser SLAM or IMU calculations to prevent being misled by drifting satellite signals. Conversely, if the doorway is cluttered with debris, resulting in low laser matching accuracy, the system will automatically reduce its reliance on laser SLAM or IMU calculations to prevent being misled by drifting satellite signals. (If the signal is large), the system will maintain its trust in weak external satellite signals or pure odometers as much as possible until laser positioning is stable.

[0031] The exception handling process and fail-safe logic address the extreme conditions mentioned in the warning message—namely, the sudden loss of the RTK signal within the buffer. Furthermore, the feature matching degree of laser SLAM is extremely low. For example, if there are a lot of temporary piles of materials at the entrance that obscure the texture of the wall, the robot cannot obtain reliable absolute observation. In this case, the system triggers the graded fault response logic.

[0032] Level 1 response: Pure inertial / odometry degraded operation, triggered by: GNSS solution not fixed and laser matching score below a set threshold. (e.g., 0.4), duration (e.g., 10 seconds).

[0033] Execution logic: The EKF filter stops observation updates and only performs the prediction step. The system enters the "track extrapolation" mode.

[0034] Control strategy: At this point, the robot's position relies entirely on IMU integration and wheel speed measurement. Due to the accumulation of errors over time, the system forcibly limits the maximum speed. The speed should be reduced to 0.5 m / s, and the adaptive adjustment of the pusher should be stopped. The pusher should remain in a retracted state, and only a straight-line passing motion should be performed to quickly leave the feature degradation region. Level 2 response: Visual assisted repositioning, triggered by: failure to restore high confidence positioning after running in DR mode for more than a distance threshold (e.g., 5 meters), or IMU divergence detection alarm.

[0035] Execution logic: Invoke the smart camera in Example 1 to activate the "artificial landmark recognition" function. Model and Scene: AprilTags or QR codes containing absolute coordinate information are pre-placed at the entrance of the cowshed (no need for full coverage, only deployed at key switching points). Visual algorithms calculate the pose of the tags relative to the camera. Combined with the global coordinates of the tag Reverse robot pose: The calculated pose is input into the EKF as a strong observation signal to reset the filter state and correct the accumulated error.

[0036] Level 3 Response: Emergency Braking and Active Exploration. Triggering Condition: All the above methods fail, and the diagonal elements of the position covariance matrix exceed the safety threshold (e.g., ...). ), Execution logic: The chassis immediately applies soft braking to stop, and reports a "positioning loss" alarm to the cloud. (Optional) Perform "spinning in place" action: The robot rotates 360 degrees in place, and the LiDAR performs an omnidirectional scan to attempt a large-scale global match with the global map. If the match is successful (score > 0.8), normal operation is restored; if it fails, the wheels are locked, and the robot waits for manual (remote dual control or on-site) takeover.

[0037] Verification of the technical effects of the embodiments: Through the implementation of the above technical solution, the pushing robot demonstrated extremely high robustness in field tests. Smoothness: During normal entry and exit from the cattle shed (buffer zone), the positioning curve remains unbroken, and the coordinate jump amplitude is controlled within... Within ( No collisions with barriers occurred during the material pushing operation due to sudden changes in coordinates.

[0038] Anti-interference capability: In the test where the RTK signal at the simulated doorway was blocked by a metal plate and the ground was covered with snow (weak laser characteristics), the dynamic weight algorithm automatically reduced the trust in the observation data, and successfully ran blindly through the 6-meter-long signal vacuum area using IMU and wheel speed meter, and quickly converged and located after entering the indoor feature-rich area.

[0039] Safety: The graded fault logic ensures that the robot can actively slow down or stop before it becomes completely lost, avoiding the risk of running wildly without control.

[0040] In summary, this embodiment, through the construction of a unified map model, the design of an adaptive EKF fusion algorithm based on signal confidence, and a comprehensive hierarchical fault handling mechanism, forms a complete and highly reliable indoor-outdoor seamless switching navigation technology solution. Those skilled in the art can configure sensor parameters and write logic code to implement the above functions based on the content disclosed in this specification, combined with the general ROS navigation stack and open-source algorithms.

[0041] Example 3 like Figure 3 As shown in the figure, this embodiment discloses in detail an adaptive edge-following material pushing control system and method that can adapt to unstructured pasture environments and has high robustness. This system not only solves the problem of accurate trajectory fitting for irregular or damaged material trough edges, but also introduces force-position hybrid control logic to solve the problems of equipment protection and flexible operation under abnormal load conditions such as encountering frozen ice blocks or high-density hay accumulation. In the actual pasture environment, feed trough retaining walls (or fences) are often broken, missing, or obstructed by cow heads protruding to feed or temporary piles of debris. This results in a large number of "outliers" in the raw point cloud data collected by lidar. If the least squares method is used directly for fitting, these outliers will severely skew the fitted curve, leading to incorrect pusher actions.

[0042] Therefore, this embodiment first establishes a feature preprocessing model based on random sampling consistency. The model is established and used in the following scenario: the robot is in a material pushing operation mode, and the 32-line LiDAR outputs point clouds at a frequency of 10Hz. The system first extracts the region of interest (ROI) on the right side of the robot's direction of travel (assuming the material trough is on the right side), with a range set to 0.5m to 3.0m laterally and 0m to 5m longitudinally in front. Within the ROI, the collected point cloud set is set as follows: ,in The RANSAC algorithm's processing flow is not a one-time computation, but rather an iterative optimization process: First, the system starts from the set... Two points are randomly selected from the data to construct a hypothetical straight line model. The general equation of this model is: Secondly, calculate the set. Euclidean distance from all remaining points in the equation to this hypothetical line The distance formula is: Explanation of physical meaning: This represents the degree to which a point in the laser point cloud deviates from the assumed edge of the trough. If this distance is less than a preset inlier threshold... (For example, if the distance is set to 0.05m, or 5cm), then the point is considered to be a real edge of the trough and is marked as an "Inlier". Conversely, if the distance is greater than the threshold, the point is determined to be a bull's head, a flying insect, or an outlier and is marked as an "Outlier". Again, the number of inliers is counted, and the system repeats the above process of "random sampling - model building - counting inliers". Number of iterations Typically set to 50-100 iterations), after all iterations, the model with the most interior points is selected as the best coarse model, and all interior points corresponding to this model are extracted to form a cleaned point cloud set. This step is particularly crucial when encountering scenarios such as "right-angle turns at the edge of the material trough (>90 degrees)" or "wall breakage". For example, at right-angle turns, RANSAC will automatically identify the long straight wall on the side where the robot is currently located by voting on the number of internal points, and eliminate interference points on the other side of the wall after the turn, thereby ensuring that the fitted object is always an effective extension of the current working surface.

[0043] Least squares exact fitting and position control model for obtaining a clean point cloud set after RANSAC cleaning Subsequently, in order to obtain a smooth and continuous feeding trajectory, this embodiment uses the least squares method for accurate curve fitting. Although RANSAC provides a coarse model, the least squares method can calculate the optimal solution with the minimum global error based on all valid interior points, and establish a straight line fitting model. (In the local coordinate system) Construct the objective function The goal is to minimize the sum of squared residuals from all interior points to the fitted line: in Let be the total number of interior points after cleaning. In order to obtain ... smallest (Slope) and (Intercept), respectively for and Find the partial derivative and set it to zero, then solve the system of equations. The physical meaning of this solution is: slope. The intercept represents the angle between the edge of the feed trough and the robot's current direction of travel (i.e., the heading deviation). This represents the lateral distance between the robot and the edge of the trough. Based on the fitting results, the system enters the position control loop, and the system sets the ideal distance between the end of the pusher plate and the edge of the trough as... (For example, 0cm, i.e., fit), calculate the lateral position error. and angle error : Explanation of physical meaning: For vehicle body width, This represents the current length of the push plate that has extended. This visually reflects whether the push board can reach the wall. This reflects whether the vehicle body is veering off course. The above error is input into the position PID controller to calculate the target extension / retraction speed of the push plate. : in The main gain for controlling the extension and retraction of the push plate is used to eliminate distance errors; This is a feedforward compensation gain, used to predict expansion and contraction trends in advance based on the direction of the wall.

[0044] The position control logic based on current / pressure feedback performs well under ideal conditions (soft feed, no obstructions). However, when encountering frozen ice, clumps of feed, or excessive accumulation, if the push plate still forcibly executes the "fitting" command, it is very easy to cause the push rod motor to stall and burn out, the mechanical structure to deform, or even the vehicle body to be pushed off course by the reaction force.

[0045] Therefore, this embodiment further introduces a force-position hybrid control strategy based on impedance control. The establishment of this model depends on the physical feedback of the actuator. For the electric actuator, the real-time phase current is collected by the Hall current sensor inside the motor driver. For hydraulic push rods, pressure values ​​are collected by a pressure sensor installed in the rodless chamber of the hydraulic cylinder. First, establish a load force observation model. Taking an electric actuator as an example, the thrust... With current They exhibit an approximately linear relationship: Explanation of physical meaning: Let be the torque constant of the motor (N·m / A). This refers to the mechanical efficiency and reduction ratio conversion factor of the lead screw drive mechanism. This is the no-load operating current. This is the estimated value of the actual resistance currently experienced by the push plate.

[0046] Based on this, a force-position hybrid control law is designed, and a safety threshold force is set for the system. (e.g., 500N) and target contact force (Typically set to 0, indicating that no additional resistance is expected), the controller's final output command (i.e., the PWM duty cycle of the pusher motor or the opening degree of the hydraulic valve) is output by the position loop. Force ring correction amount A joint decision.

[0047] This embodiment employs a compliant control strategy based on admittance, the core formula of which is as follows: For ease of engineering implementation, it is simplified to a discrete-domain correction logic:

[0048] Explanation of physical meaning: It is the theoretical extension length calculated solely based on laser data (for wall contact). It is the trigger force threshold for entering the flexible avoidance mode. It is the force feedback gain coefficient (also known as "compliance"). The logic of this formula is: when the push plate experiences resistance... When the force is below the threshold (such as when pushing ordinary loose hay), the force correction term is 0, and the system operates entirely according to position control to ensure a clean push; when the pusher plate hits frozen ice or hard objects, resulting in resistance... Exceed At that time, force correction term The resistance increases rapidly, and this term is subtracted from the theoretical extension length, which commands the push plate to actively retract. The greater the resistance, the more it retracts until the resistance drops below the threshold. This behavior is as if the push plate is equipped with a "virtual spring". When it encounters a hard object, it will automatically give way, which protects the motor and ensures that the vehicle can continue to drive smoothly through the obstacle area. After passing through (the resistance disappears), the push plate will extend again to fit against the wall under the action of the position ring.

[0049] Based on the above perception and control model, the specific operation process and abnormal condition response are as follows: The robot travels along the cattle shed passage at a speed of 15km / h, and the lidar continuously scans.

[0050] Data cleaning stage: The RANSAC algorithm removes bullheads (outliers) and noise points behind broken walls in the point cloud in real time, and outputs high-quality edge inliers.

[0051] Trajectory planning stage: The least squares method is used to fit a smooth edge line of the feed trough, and the theoretically required extension length of the pusher plate is calculated. The intention is to make the rubber strip of the push plate press tightly against the ground of the material trough.

[0052] Condition monitoring phase: The current sensor samples the motor current at a frequency of 1kHz to calculate the real-time thrust. .

[0053] Hybrid control decision-making phase: Scenario A (normal material pushing): Detection detected (If the value is less than the threshold of 300N), it is determined to be loose feed, and the controller outputs... The pusher plate remains close to the edge, pushing the feed back to the feeding area. Scenario B (encountering ice): The pusher plate suddenly hits a frozen ice pile on the ground, causing the current to surge instantly. Calculations are needed. At this point, the force control logic intervenes and calculates: The controller immediately instructs the push plate to retract 25mm from its theoretical position. Scenario C (continuous overload): If the resistance is not eliminated after retraction (e.g., jamming), and Continuously exceeding safety limits (e.g., 1500N) If the time exceeds 500ms, the system triggers "overload protection," the pusher plate retracts completely, and a "foreign object obstruction" work order is reported to the cloud. Simultaneously, the robot detours around the area. Scenario D (cliff-like gap): When the laser detects that the edge of the material trough has completely disappeared (insufficient points in RANSAC), the position loop fails. At this time, the system automatically locks the current length of the pusher plate and maintains straight-line travel until a valid edge is detected again, preventing the pusher plate from overextending and colliding with unknown objects due to loss of reference. Through the above complete technical solution, this embodiment realizes an intelligent material pushing control that has both "visual (laser) precision" and "tactile (force control) flexibility", which fundamentally solves the problems of equipment wear and uneven operation caused by rigid operations in unstructured pastures. Based on the algorithm flow, mathematical model and parameter settings disclosed in this specification, combined with general industrial control hardware (such as STM32 or PLC) and sensors, those skilled in the art can realize this adaptive material pushing function in actual products.

[0054] Example 4 like Figure 4As shown in the figure, this embodiment discloses in detail a highly robust environmental perception and obstacle avoidance system applied to complex scenarios in animal husbandry. Addressing the technical pain points of uncontrollable lighting conditions in pasture environments (from strong light to complete darkness) and high similarity in the shapes of work objects (lying cows and feed piles have similar outlines), this embodiment constructs a semantic recognition model with "RGB-I multimodal YOLOv5" as the core and innovatively designs a "biosign micro-motion detection" verification mechanism based on the spatiotemporal variance of laser point clouds. Combined with hierarchical decision-making logic, it realizes intelligent operation with all-weather and zero-misoperation capabilities.

[0055] To address the issue of conventional vision solutions failing at night or in low-light cattle sheds, this embodiment abandons the traditional input mode that relies solely on RGB images and constructs a four-channel input model that integrates lidar reflectivity (intensity) information.

[0056] Dataset Construction and Preprocessing: First, a ranch-specific multimodal dataset was constructed. Data collection vehicles collected data at different time periods (covering noon, dusk, and late night) and under different weather conditions (sunny, rainy, and snowy). The collection equipment included the 32-line LiDAR and binocular intelligent camera described in Example 1. By jointly calibrating parameters, the 3D point cloud of the LiDAR was projected onto the 2D image plane of the camera. The projection model followed the pinhole camera model equations. Explanation of physical meaning: It is a homogeneous coordinate point in the lidar coordinate system; It is an extrinsic parameter matrix (rotation and translation) that describes the relative positions of the radar and the camera; It is the camera intrinsic parameter matrix, including focal length. and optical center coordinates ; These are the pixel coordinates projected onto the image; Using the scale factor (depth), and leveraging the aforementioned projection relationship, the "reflectivity intensity" of the laser point cloud falling within the image's field of view is extracted. Since different objects have different reflectivity characteristics (e.g., dry hay typically has higher reflectivity at 905nm wavelength than cow hair or damp ground), reflectivity provides a physical characteristic unaffected by ambient light. The system generates a single-channel "intensity grayscale image" with the same resolution as the RGB image, which is then overlaid on the RGB image as the fourth channel to form... The tensor input contains five core categories: Feed_Pile, Cow_Standing, Cow_Lying, Human, and Vehicle.

[0057] The improved YOLOv5 network architecture design model is based on the YOLOv5s architecture. To enhance the ability to distinguish between feed textures and biological fur textures, a CBAM attention mechanism module is introduced at the end of the CSPDarknet backbone network. CBAM includes two sub-modules: Channel Attention and Spatial Attention. The formula for calculating Channel Attention is as follows: Explanation of physical meaning: Using the input feature map, this formula extracts features through parallel average pooling and max pooling, which are then fused by a multilayer perceptron (MLP) and finally fused using the sigmoid function. Weight coefficients are generated for each channel, which allows the model to automatically "focus" on the 4th channel containing reflectivity information or the RGB channel containing specific textures. At night, the weight of the RGB channel is automatically reduced, and the weight of the reflectivity channel is increased. The loss function CIoULoss is used to solve the convergence problem when the overlap between the predicted box and the ground truth box is low. in For Euclidean distance, The minimum diagonal length of the bounding rectangle covering both boxes ensures that the model can not only classify accurately but also output high-precision bounding boxes, providing accurate distance references for subsequent obstacle avoidance.

[0058] In actual operation, the multimodal adaptive perception logic for nighttime and low-light environments switches perception modes in real time based on the ambient light intensity sensor (or the gain value of the camera's ISP). It fuses visual and geometric probabilities through a Bayesian inference framework. When the ambient light level is below a threshold (e.g., 5 Lux, entering night mode), the RGB image is almost completely black, and the confidence level of pure visual detection drops significantly. At this point, the system activates the "laser-dominated - vision-assisted" logic. ROI generation: First, obstacle detection based on Euclidean clustering is performed using LiDAR to generate several 3D candidate bounding boxes.

[0059] Reflectance feature extraction: Calculate the average reflectance of all point clouds within each candidate bounding box. and reflectivity variance .

[0060] Physical criterion: The average reflectance of the feed pile (hay / silage) based on pre-statistical prior data. Typically distributed in the interval (Normalized values), and variance Larger (rougher); while the black patches on Holstein cows have extremely low reflectivity ( The white patches have higher reflectivity, which leads to a higher variance in their reflectivity. It exhibits a bimodal distribution.

[0061] Fusion classification decision: The geometric features (aspect ratio, volume) and reflectivity features extracted by laser are input into a lightweight fully connected neural network (MLP), which outputs a preliminary classification probability. Meanwhile, the visual classification probability output by YOLOv5 is Final classification confidence Dynamic weighted fusion is adopted: Among them, weight It is positively correlated with ambient brightness; at night, , This ensures that even in complete darkness, the robot can still correctly distinguish between "fodder" and "cattle" in front of it by using lidar to perceive the shape and material (reflectivity) of objects.

[0062] For extreme negative sample scenarios where "lying cows" and "irregular haystacks" are extremely similar in morphology and difficult to distinguish when stationary, this embodiment introduces a second verification mechanism based on biological micro-movements. This mechanism utilizes the unavoidable breathing fluctuations and rumination movements of living organisms (cows) to physically distinguish them from absolutely stationary haystacks.

[0063] When the spatiotemporal micro-motion detection model is established, and the primary semantic recognition (YOLO + reflectivity) determines that the target in front is likely a "lying cow" (a fuzzy area with a confidence level between 0.4 and 0.7), the robot automatically decelerates to 0.2 m / s or pauses briefly, activates "staring mode," and continuously collects point cloud data of the target area. Seconds (e.g., 1.0 second, i.e., 10 frames of laser data), construct a spatiotemporal voxel mesh, and divide the target point cloud into small voxels (e.g., ... For each voxel Track the number of point clouds contained within it. and the position of the centroid of the point cloud Over time The variation is used to define a "respiratory characteristic index"—radial distance variance. Since cow respiration causes a periodic displacement of approximately 2-5 cm on the surface of the chest and abdomen, the distance of the target surface point cloud centroid relative to the radar origin is calculated. variance: Explanation of physical meaning: , The variance of the average distance within the observation window, for a stationary haystack, is limited by radar ranging noise. Extremely small (usually) For a cow that is breathing, the variance of the distance to points on its body surface is... It will be significantly higher than the system noise floor (usually) ), Frequency domain analysis (optional enhancement): If the time domain variance characteristics are not obvious, further analysis of the distance sequence can be performed. Perform Fast Fourier Transform (FFT): Does the detection spectrum contain frequencies? (Right now The energy peak value within the range of the breathing frequency of an adult dairy cow is used to determine whether the object has "life characteristics" if a significant low-frequency energy peak is detected.

[0064] The dual verification logic flow verification system executes the following logic: Step 1: The YOLO model detected a low-lying object in front of it, classifying it as Cow_Lying, but the confidence level was low. Step 2: The system extracts the ROI point cloud corresponding to the object and calculates its "radial distance variance" within 1 second. .

[0065] Step 3: Judgment, if (Biological threshold), if confirmed as "lying cow", obstacle avoidance is triggered. (Noise threshold) Determine YOLO false detection, correct the target to "hay pile", and perform the pushing operation. This mechanism is like equipping the robot with a "stethoscope", which effectively prevents collisions with reclining cows (if misjudged as grass) or false obstacle avoidance of hay piles (if misjudged as cows) caused by visual misjudgment. Based on the aforementioned high-precision semantic perception results, the hierarchical multi-level obstacle avoidance control strategy implements a hierarchical control strategy to balance operational efficiency and safety.

[0066] Level 1 Strategy: Semantic Bypass When the identification confirms Cow_Standing or Cow_Lying, and the distance is... At this time, the path planning layer marks the area as a high-cost zone in the local cost map. The robot then uses the TimedElasticBand (TEB) algorithm to plan a smooth detour curve with a detour spacing of 0.5m, which minimizes disturbance to the cows and reduces the unpushed area as much as possible. Secondary strategy: Geometric Braking. If semantic recognition fails (e.g., due to an untrained, irregularly shaped obstacle), but the LiDAR detects a height ahead... For protrusions that fail the "biological micro-motion detection" (i.e., inanimate objects that cannot be pushed, such as a dropped shovel), the system determines the obstacle's distance based on the obstacle's position. Perform segmented deceleration: When the distance is less than (e.g., at 1.0m), the speed drops to 0. Level 3 Strategy: Blind Spot Emergency Stop. When the ultrasonic radar detects an object within 0.5m of the vehicle (regardless of its semantics, it could be a suddenly approaching obstacle), the lower-level controller directly cuts off power output and applies the brakes until the obstacle is removed. In summary, this embodiment constructs an RGB-I four-channel YOLO model, combines a nighttime reflectivity weighted algorithm with innovative biometric micro-motion verification technology, and forms a complete all-weather environmental perception solution. This solution not only solves the problem of "seeing" but also the problem of "understanding," enabling the material-pushing robot to achieve truly intelligent autonomous operation in complex pasture environments where people, vehicles, cattle, and grass are mixed. Those skilled in the art can train and deploy the corresponding algorithm by referring to the model structure, fusion formula, and verification logic of this embodiment. Example 5 like Figure 5 As shown, this embodiment discloses in detail an all-weather motion control system that integrates active anti-slip path planning, underlying traction control, and intelligent escape strategy. In response to the common winter conditions of snow-covered roads, muddy and potholed areas during the spring frost season, and steep slopes across cattle shed passages, this embodiment fully utilizes the hardware advantages of the robot's "four-wheel independent drive" drive-by-wire chassis. Through a multi-level algorithm architecture, it solves core technical challenges such as vehicle sideslip instability, wheel spin and getting stuck, and odometer positioning failure on low-traction surfaces.

[0067] Road Friction Coefficient Estimation and Active Anti-skid Planning Model Based on Multidimensional Perception: Traditional mobile robot path planning typically assumes a constant road friction coefficient, which can lead to severe skidding accidents on icy or snowy roads. This embodiment first establishes a road friction coefficient estimation and active anti-skid planning model based on semantic perception and historical data. Dynamic friction coefficient layer model.

[0068] Model Establishment and Usage Scenarios: This model is embedded between the robot's global path planner and local path planner.

[0069] The scenario is as follows: A robot is preparing to navigate an outdoor curve. Part of the road surface is covered in snow, while other areas are icy. First, the system uses the semantic recognition results from Example 4 to assign material attributes (e.g., cement road, compacted snow, dark ice, mud) to each grid cell in the environmental mesh map. Second, it establishes a road surface friction coefficient mapping table. Based on prior experimental data: dry cement road compact the snow. Icy road surface To improve estimation accuracy, the system introduces a "posterior correction" mechanism: using the wheel slip rate data from the previous passage through the area, the actual equivalent friction coefficient is calculated backward and the map layer is updated.

[0070] After obtaining the road surface friction coefficient distribution, the speed planning mathematical model based on friction circle constraints introduces dynamic friction circle constraints when performing local path planning (TEB algorithm). When the vehicle is driving on a curve, the adhesion between the tire and the ground must simultaneously meet the requirements of longitudinal drive and lateral centripetal force. The constraint inequalities are as follows: Converting the force into kinematic parameters yields the constraint relationship between the maximum safe velocity and the curvature: Explanation of physical meaning: It is a longitudinal traction force or braking force. The lateral force (centripetal force) required to maintain steering; For the vertical load of the tire (i.e. ); For vehicle linear velocity, The radius of curvature of the path. For longitudinal acceleration, this formula clarifies the physical boundary: at the coefficient of friction On ice surfaces with extremely low curvature (e.g., 0.1), if the radius of curvature of the planned path is... For smaller (sharp turns), the speed must be reduced drastically. Alternatively, if the speed cannot be reduced, the radius of the planned path can be forcibly increased. (Walk a large circle).

[0071] Specific implementation logic: When the upper-level planner detects the road segment ahead... When there is snow accumulation, the algorithm automatically triggers the regeneration of the "VelocityProfile". The system no longer uses the default trapezoidal velocity curve, but instead adopts an S-shaped restricted velocity curve and sets the maximum permissible lateral acceleration. For each waypoint Its maximum passing speed Strictly limited to: If the planned speed exceeds this value, the system will plan the deceleration phase in advance to ensure that the robot has decelerated before entering the slippery curve, thus "proactively" eliminating the risk of skidding at the planning level, rather than passively trying to save the vehicle after skidding occurs.

[0072] In terms of slip ratio control and torque vector distribution for four-wheel independent drive, at the execution level, this embodiment designs a distributed traction control system based on the optimal slip ratio for the four-wheel independent drive motor.

[0073] The real-time slip ratio calculation model for the first One wheel ( ), in fact, the slip ratio Defined as: Explanation of physical meaning: This refers to the angular velocity fed back by the encoder of the wheel motor. The tire's rolling radius, For vehicle reference speed, the key technical point is: how to obtain the true speed when all four wheels are slipping. ; This embodiment uses the fused value of RTK-GNSS velocity and IMU integral velocity as... When RTK fails, the speed of the "non-driving wheel" or the "wheel with the lowest slip ratio" is used as a reference; if all four wheels slip, the short-term speed is calculated entirely based on the IMU acceleration integral. Based on the PID-based torque regulation algorithm, and considering tire mechanics, the system provides maximum longitudinal adhesion when the slip ratio is controlled within the 15%-20% range on snow. An independent sliding mode variable structure PID controller is designed for each wheel. When a wheel slip ratio is detected... The target torque of the wheel during (excessive slippage) Revised to: in (Optimal slip ratio) At the same time, taking advantage of the four-wheel independent control, the system executes a torque vector distribution strategy. When the left wheel is stuck on the ice and slips, while the right wheel is on the concrete, the system not only reduces the torque on the left wheel to suppress slippage, but also actively increases the torque on the right wheel. It uses yaw moment to assist the vehicle in maintaining straight-line driving and prevents the front of the vehicle from turning due to slippage on one side.

[0074] The intelligent traction mode for muddy and potholed roads is designed for the common "getting stuck" scenario during the pasture's frost-covering period—that is, one or more wheels are suspended in the air / stuck in the mud, with a slip rate of 100%. Conventional TCS can only cut off power, causing the vehicle to stall. This embodiment designs an traction algorithm based on "electronic differential lock" and "creep swing".

[0075] The electronic differential lock logic will activate when the system detects the following state for more than 2 seconds: actual vehicle speed. However, the rotational speed of a certain wheel (Spinning freely), and the IMU detects an abnormal vehicle tilt angle (Pitch / Roll) (indicating being stuck in a pothole), the system automatically activates the "get-out-of-trouble mode." First, it implements the electronic differential lock logic: forcibly applying a large braking torque to the spinning wheel. Or the driving torque allocated to that wheel It dropped to 0.

[0076] Based on the principles of energy conservation and power balance, unused power is fully allocated to the remaining wheels with traction, giving the non-stuck wheels several times the driving torque under normal operating conditions, simulating the effect of a mechanical differential lock, and forcibly pulling the vehicle out of the mud pit.

[0077] If simple torque redistribution fails to extricate the vehicle from a stuck situation (e.g., all four wheels are stuck in deep snow), the variable frequency oscillation "creep sway" algorithm activates a "creep sway" strategy. This strategy mimics the "back-and-forth swaying" motion of an experienced driver, driving the motor to output a sinusoidal torque that varies with time. Explanation of physical meaning: Based on the bias torque; The oscillation amplitude (close to the maximum torque of the motor); With an oscillation frequency (typically set between 0.5Hz and 2Hz), this alternating torque causes the tires to roll back and forth slightly. Utilizing the principle that static friction is greater than dynamic friction, it seeks the momentary peak of grip. Simultaneously, the alternating torque compacts the loose snow beneath the tires, creating a temporary "starting platform," which, combined with the vehicle pitch angle detected by the IMU, helps to establish this platform. The algorithm automatically adjusts the phase of the sine wave: when the car body slides down with gravity, the torque is increased, and when it slides down against gravity, the torque is decreased. The resonance effect generated by the gravitational potential energy is used to "shake" the vehicle out of potholes.

[0078] In the above-mentioned intense anti-skid and get-out process, the wheel odometer completely fails, and the laser SLAM may also make matching errors due to violent shaking. This embodiment maintains the positioning without loss by improving the covariance update logic of the error state Kalman filter (ES-EKF).

[0079] In the standard EKF, the adaptive observation noise matrix is ​​designed as follows: Typically fixed, this embodiment constructs a system based on slip ratio. Adaptive function: Physical meaning: This represents the level of "distrust" in odometer data. As the adjustment coefficient, when the slip ratio hour, The system trusts the odometer; when (When slipping severely) The system completely disables the odometer's correction function. 2. Pure inertial navigation and ZUPT correction: When all four wheels are detected to be slipping and the vehicle is attempting to get out of trouble (at extremely low speed), the filter enters "pure inertial navigation" mode, and position updates rely solely on IMU integration. To suppress IMU integral drift, the system utilizes momentary pauses during the escape process (e.g., the sinusoidal torque crossing zero, or the instant of commutation) to perform zero-velocity correction. The logical criterion is: if the accelerometer variance... And gyroscope variance If the vehicle is determined to be momentarily stationary, the "speed is 0" is input into the EKF as strong observation information to forcibly pull back the divergent speed state quantity. In this way, during the tens of seconds of struggling to get out of trouble, the positioning error is kept within the meter range, ensuring that path tracking can be automatically restored after the vehicle is successfully freed.

[0080] In summary, this embodiment introduces friction circle constraints at the planning layer, utilizes four-wheel independent drive to achieve torque vector distribution at the control layer, and applies a biomimetic "rocking" escape algorithm in extreme scenarios, combined with an adaptive positioning strategy, to form a complete all-weather extreme road condition motion control scheme. This not only solves the problem of "driving steadily" but also addresses the reliability problem of "getting out" in unattended situations. Those skilled in the art can implement the above algorithm using a chassis platform equipped with four-wheel hub motors or independent drive axles, based on this specification.

[0081] It should be noted that, in this document, relational terms such as "first" and "second" are used only to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Moreover, the terms "comprising," "including," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such process, method, article, or apparatus.

[0082] Although embodiments of the invention have been shown and described, it will be understood by those skilled in the art that various changes, modifications, substitutions and alterations can be made to these embodiments without departing from the principles and spirit of the invention, the scope of which is defined by the appended claims and their equivalents.

Claims

1. An autonomous navigation method for a livestock feeding robot based on lidar, applied to an unmanned feeding robot, wherein the robot is equipped with a 32-line mechanical lidar, a binocular camera, an inertial measurement unit, a global navigation satellite system receiver, and an automotive-grade drive-by-wire chassis; characterized in that, The method includes: Step 1: Construct a hard-triggered synchronization mechanism based on field-programmable gate arrays, align all sensor data to the second pulse time axis of the global navigation satellite system, and establish a hard-coupled hybrid map model that includes geographic coordinate system, local odometry coordinate system and laser map coordinate system. The real-time latitude and longitude are projected into the laser map coordinate system through the static transformation matrix calculated by offline mapping. Step 2: Construct an extended Kalman filter model based on error state. When the robot is in the outdoor-to-indoor transition buffer zone where satellite signals are limited, it does not directly switch the observation source. Instead, it calculates the ratio of the observation covariance of the global navigation satellite system to the laser positioning matching degree in real time, and generates dynamic weight coefficients accordingly. It then constructs a virtual fusion observation value containing satellite positioning points and laser matching points and inputs it into the filter model. Step 3: Based on the obtained fused pose, the reflectivity intensity and geometric distribution characteristics of the lidar point cloud are used to simultaneously identify the edge curve of the trough and the biological signs in front, and output the edge extension and retraction command of the pusher and the obstacle avoidance movement command of the chassis.

2. The autonomous navigation method for a livestock feeding robot based on lidar according to claim 1, characterized in that, In the adaptive fusion positioning step, the generation logic of the dynamic weight coefficient is as follows: establish a distance attenuation function that changes in an S-shaped curve as the robot enters the buffer zone; calculate in real time the first variance corresponding to the horizontal accuracy factor of the satellite signal and the second variance corresponding to the trace of the inverse Hessian matrix of the laser point cloud normal distribution transformation matching algorithm; multiply the distance attenuation function by the signal confidence factor to obtain the final weight coefficient, wherein the signal confidence factor is negatively correlated with the first variance and positively correlated with the second variance.

3. The autonomous navigation method for a livestock feeding robot based on lidar according to claim 1, characterized in that, In the unified spatiotemporal reference step, the hard-triggered synchronization mechanism is specifically configured as follows: using a field-programmable gate array as the core controller, synchronizing the lidar with precise time protocol network messages, triggering the exposure of the binocular camera with physical level pulses, and responding to the inertial measurement unit data with external interrupts, thereby limiting the time synchronization error of the multi-source sensors to the microsecond range.

4. The autonomous navigation method for a livestock feeding robot based on lidar according to claim 1, characterized in that, The method also includes an online automatic extrinsic parameter calibration process: when the robot is in a non-degenerate motion excitation state, a nonlinear least squares optimization objective function is constructed. The objective function aims to minimize the residual between the relative pose trajectory calculated by the laser odometry and the relative pose trajectory calculated by the visual inertial odometry in the rigid body transformation closed loop, thereby iteratively solving the static transformation matrix of the camera relative to the lidar.

5. The autonomous navigation method for a livestock feeding robot based on lidar according to claim 1, characterized in that, In the operation and motion control steps, the following strategy is adopted for the identification of the material trough edge and the control of the push plate: In the point cloud of the captured region of interest, the random sampling consensus algorithm is used to remove outliers that belong to the noise, and the cleaned set of inner points of the edge is obtained; the least squares method is used to linearly fit the inner point set to obtain the heading deviation and lateral distance deviation; based on the deviation, the extension and retraction speed of the push plate is adjusted by a proportional-derivative controller.

6. The autonomous navigation method for a livestock feeding robot based on lidar according to claim 5, characterized in that, The push plate control also introduces a force-position hybrid control logic based on the admittance model: real-time monitoring of the load feedback value of the push plate drive mechanism; when the load feedback value exceeds the set safety threshold, a reverse position correction amount proportional to the load excess value is generated and superimposed on the theoretical extension position command of the push plate, so that the push plate exhibits flexible retraction characteristics.

7. The autonomous navigation method for a livestock feeding robot based on lidar according to claim 1, characterized in that, The identification of biological signs in front adopts a dual verification logic: the first verification is to project the reflectivity intensity of the laser point cloud onto the visual image to generate a four-channel tensor, and then use a deep learning model to distinguish between biological and non-biological targets. The second verification is for suspected stationary biological targets. The variance of the radial distance of the centroid of its point cloud within a preset time window is calculated. When the variance is greater than a preset respiratory micro-movement threshold, the target is determined to be biologically active.

8. The autonomous navigation method for a livestock feeding robot based on lidar according to claim 1, characterized in that, The obstacle avoidance motion command generation of the chassis includes active anti-skid planning based on friction circle constraints: a dynamic friction coefficient layer is established in the environmental map, and dynamic constraints are introduced during path planning to limit the maximum planned speed at the path point, so that the resultant force of the longitudinal traction force and the lateral centripetal force required by the vehicle at the path point does not exceed the maximum adhesion force that the current road friction coefficient can provide.

9. The autonomous navigation method for a livestock feeding robot based on lidar according to claim 1, characterized in that, The chassis motion control adopts a torque vector distribution strategy with independent four-wheel drive: the slip ratio of each wheel is calculated in real time, and when the slip ratio exceeds the optimal adhesion range, the torque of the slipping wheel is reduced by the slip mode controller, while the torque of the non-slipping wheel is increased at the same time to generate a yaw moment to correct the body posture.

10. The autonomous navigation method for a livestock feeding robot based on lidar according to claim 9, characterized in that, The method also includes a variable frequency oscillation traction control mode: when the vehicle is detected to be in trouble and the wheels are spinning, the drive motor is controlled to output a sinusoidal torque that includes a basic bias and oscillation amplitude, and the frequency and phase of the sinusoidal torque are dynamically adjusted in combination with the vehicle pitch angle phase fed back by the inertial measurement unit, so as to increase the instantaneous grip of the tires by utilizing the resonance effect.

Images