CN-121994237-A - Laser radar-based autonomous navigation method for animal husbandry pusher robot

CN121994237ACN 121994237 ACN121994237 ACN 121994237ACN-121994237-A

Abstract

The invention relates to an agricultural robot and discloses an autonomous navigation method of a stock raising pushing robot based on a laser radar, which comprises a microsecond level multi-sensor time synchronization framework based on an FPGA (field programmable gate array), wherein in a navigation layer, a virtual fusion observation value is constructed to realize seamless switching of GNSS and laser SLAM (laser beam cross-section), in a perception layer, an RGB-I multi-mode model fusing laser reflectivity and a biological micro-motion detection technology are adopted to accurately distinguish feed from biological barriers, and in an execution layer, the flexible self-adaptive pushing is realized by combining trough edge fitting and force-position mixed control.

Inventors

SHA YAOYAO
DENG YU
ZHENG KUI

Assignees

经纬达智能科技(南京)有限公司

Dates

Publication Date: 20260508
Application Date: 20260129

Claims (10)

1. The autonomous navigation method of the animal husbandry pushing robot based on the laser radar is applied to an unmanned pushing robot, the robot is provided with a 32-wire mechanical laser radar, a binocular camera, an inertial measurement unit, a global navigation satellite system receiver and a vehicle-scale wire control chassis, and is characterized by comprising the following steps: Step 1, constructing a hard trigger synchronization mechanism based on a field programmable gate array, aligning all sensor data to a second pulse time axis of a global navigation satellite system, building a hard coupling hybrid map model comprising a geographic coordinate system, a local odometer coordinate system and a laser map coordinate system, and projecting real-time longitude and latitude into the laser map coordinate system through a static transformation matrix calculated by offline mapping; Step 2, an extended Kalman filtering model based on an error state is constructed, when the robot is positioned in an outdoor-indoor transition buffer with limited satellite signals, an observation source is not directly switched, the ratio of the observation covariance of the global navigation satellite system to the laser positioning matching degree is calculated in real time, a dynamic weight coefficient is generated according to the ratio, and a virtual fusion observation value comprising satellite positioning points and laser matching points is constructed and input into the filtering model; and 3, on the basis of the obtained fusion pose, utilizing the reflectivity intensity and geometric distribution characteristics of the laser radar point cloud to simultaneously identify the edge curve of the trough and the biological sign in front, and outputting the welting telescopic instruction of the push plate and the obstacle avoidance movement instruction of the chassis.
2. The autonomous navigation method of the animal husbandry pusher robot based on the laser radar according to claim 1, wherein in the self-adaptive fusion positioning step, the generation logic of the dynamic weight coefficient is that a distance attenuation function of the distance of a random robot entering a buffer area and changing in an S-shaped curve is established, a first variance corresponding to a horizontal precision factor of a satellite signal and a second variance corresponding to a trace of a hessian matrix inverse matrix of a laser point cloud normal distribution transformation matching algorithm are calculated in real time, and the distance attenuation function is multiplied by a signal confidence factor to obtain a final weight coefficient, wherein the signal confidence factor is in negative correlation with the first variance and in positive correlation with the second variance.
3. The autonomous navigation method of the animal husbandry pusher robot based on the laser radar according to claim 1, wherein in the space-time reference unifying step, the hard trigger synchronization mechanism is specifically configured to synchronize the laser radar by using a field programmable gate array as a core controller, triggering the exposure of the binocular camera by using a physical level pulse, and limiting the time synchronization error of the multi-source sensor within a microsecond range by using an external interrupt response inertial measurement unit data.
4. An autonomous navigation method of a laser radar based animal husbandry pusher robot according to claim 1, further comprising an on-line off-reference automatic calibration process of constructing a nonlinear least squares optimization objective function with the robot in a non-degenerate motion excited state, the objective function being designed to minimize residuals of laser odometer calculated relative pose trajectories and visual inertial odometer calculated relative pose trajectories in a rigid body transformation closed loop, thereby iteratively solving a static transformation matrix of the camera relative to the laser radar.
5. The autonomous navigation method of the animal husbandry pusher robot based on the laser radar according to claim 1, wherein in the operation and motion control step, the identification and pushing plate control of the trough edge adopts a strategy that in the intercepted point cloud of the interested area, the local and external points belonging to outlier noise are removed by utilizing a random sampling consistency algorithm to obtain a cleaned edge internal point set, the internal point set is subjected to linear fitting by adopting a least square method to obtain course deviation and transverse distance deviation, and the expansion and contraction speed of the pushing plate is regulated by a proportional-differential controller based on the deviation.
6. An autonomous navigation method of a laser radar based animal husbandry pusher robot according to claim 5, wherein the pusher control further incorporates admittance model-based force-position hybrid control logic for monitoring the load feedback value of the pusher drive mechanism in real time, and when the load feedback value exceeds a set safety threshold, generating a reverse position correction amount proportional to the load overload value, and superimposing the reverse position correction amount on a theoretical extended position command of the pusher, so that the pusher exhibits a flexible retraction characteristic.
7. The autonomous navigation method of the animal husbandry pusher robot based on the laser radar according to claim 1, wherein the identification of the front biological sign adopts double verification logic, wherein the first double verification is to project the reflectivity intensity of laser point cloud to a visual image to generate a four-channel tensor, and the biological and non-biological targets are distinguished through a deep learning model, the second double verification is to count radial distance variance of the point cloud centroid of the suspected stationary biological targets in a preset time window, and when the variance is larger than a preset respiratory jog threshold, the target is judged to have biological activity.
8. An autonomous navigation method of a laser radar-based animal husbandry pusher robot according to claim 1, wherein the generation of obstacle avoidance motion instructions of the chassis comprises an active anti-slip plan based on friction circle constraint, wherein a dynamic friction coefficient layer is built in an environment map, dynamic constraint conditions are introduced during path planning, and the maximum planning speed of a path point is limited, so that the resultant force of longitudinal traction force and transverse centripetal force required by a vehicle at the path point does not exceed the maximum adhesive force provided by the current road friction coefficient.
9. The autonomous navigation method of the livestock pusher robot based on the laser radar according to claim 1, wherein the motion control of the chassis adopts a torque vector distribution strategy of four-wheel independent driving, wherein slip rate of each wheel is calculated in real time, and when the slip rate exceeds an optimal attachment interval, the torque of the slipping wheel is reduced through a slip mode controller, and simultaneously, the torque of the non-slipping side wheel is increased to generate yaw moment for correcting the body posture.
10. The autonomous navigation method of the livestock pusher robot based on the laser radar according to claim 9, wherein the method further comprises a variable frequency oscillation escape control mode, wherein when the vehicle is detected to be in a dilemma and the wheels idle, the driving motor is controlled to output sine wave torque comprising basic bias and oscillation amplitude, and the frequency and the phase of the sine wave torque are dynamically adjusted in combination with the pitch angle phase of the vehicle body fed back by the inertial measurement unit, so that the instant ground grabbing force of the tire is increased by utilizing the resonance effect.

Description

Laser radar-based autonomous navigation method for animal husbandry pusher robot Technical Field The invention relates to an agricultural robot, in particular to an autonomous navigation method of an animal husbandry pusher robot based on a laser radar, Background Along with the improvement of the intensive degree of animal husbandry, TMR (total mixed ration) feeding technology is popular, and during the feeding process, cows can arch feed out of a feeding area, so that feed needs to be pushed back by pushing operation, the current pushing operation mainly depends on manual driving of diesel vehicles or early simple automatic equipment, but in practical application, the current navigation and control technology faces a specific technical bottleneck, is difficult to meet all-weather and unmanned operation demands, Firstly, aiming at the problem of special indoor and outdoor mixed scene navigation of pasture, the prior art usually adopts a single navigation mode, for example, an outdoor robot adopting a GNSS (global navigation satellite system) cannot be positioned in a cowshed with a totally-enclosed metal ceiling, while an AGV only relying on a 2D laser radar is easy to generate positioning loss when entering an outdoor open connection channel or encountering rain and snow weather due to sparse environmental geometric characteristics or severe road texture change, in the prior art, when the indoor and outdoor transition areas are treated, hard switching logic is often adopted (namely, a sensor source is directly switched at a certain point), the jump and time delay of sensor data at the switching moment are ignored in the mode, the position oscillation, sudden stop and even collision of the robot at the entrance of the cowshed are easy to occur, and smooth continuous operation cannot be realized, Secondly, in terms of identification and obstacle avoidance of operation objects, the prior art lacks understanding capability of environmental semantics, the traditional obstacle avoidance system is mostly based on the ranging principle of infrared or ultrasonic waves, all front protrusions are uniformly regarded as obstacles, however, a 'feed pile' (an object to be pushed) and a 'obstacle' (such as a prone cow, a worker and a tool) naturally exist on the operation path of a pusher robot, the prior art cannot distinguish the feed pile from the obstacle, so that the robot frequently misjudges the forage to be pushed as the obstacle and frequently stops, or in the case of being incapable of distinguishing biological signs, collision risks are caused to stationary and prone cows, especially under the conditions of insufficient illumination and visual failure at night, the contradiction is more prominent, In addition, in the aspect of pushing execution control, the prior equipment mostly adopts fixed track tracking and rigid control, a trough retaining wall of a pasture is not an ideal straight line, construction errors, breakage or bending often exist, a robot running along the fixed track cannot be self-adaptively attached to the edge of the trough, so that the pushing is not clean or hard friction is generated between a pushing plate and a wall body, in addition, frozen ice cubes are often mixed in a feed pile in a severe cold environment in winter, the prior position control mode lacks a force feedback mechanism, the pushing plate still stretches out forcibly when encountering hard object blocking, the overload burning of a motor or the damage of a mechanical structure is extremely easy to be caused, Finally, pasture road surface environment is complicated, contains wet smooth cement ground, muddy soil road and snow road surface, and the motion planning of traditional wheeled robot does not consider the dynamic change of road surface coefficient of friction, when carrying out the turn or add and slow down, skids out of control because of the adhesion is not enough easily, and then leads to the odometer error to diverge, influences navigation accuracy. Disclosure of Invention The invention aims to provide an autonomous navigation method of a stock raising pushing robot based on a laser radar, so as to solve the problems in the background technology, In order to achieve the above object, the present invention provides an autonomous navigation method of an animal husbandry pushing robot based on a laser radar, which is applied to an unmanned pushing robot, wherein the robot is configured with a 32-line mechanical laser radar, a binocular camera, an inertial measurement unit, a global navigation satellite system receiver and a vehicle-gauge-level drive-by-wire chassis; The method comprises the following steps: Step 1, constructing a hard trigger synchronization mechanism based on a field programmable gate array, aligning all sensor data to a second pulse time axis of a global navigation satellite system, building a hard coupling hybrid map model comprising a geographic coordinate system, a local odometer coordinate sy