A sow farrowing house inspection robot and an inspection method thereof

Abstract

The application provides a sow delivery room inspection robot and an inspection method thereof. The inspection robot comprises an inspection chassis, a navigation control box and an inspection camera unit. The inspection method adopts a laser radar SLAM to construct a 2D grid map, and combines AMCL positioning and A* path planning to realize autonomous navigation. After reaching the inspection point, the inspection camera pose is adjusted by controlling the lifting type push rod and the rudder cloud platform to shoot the sow vulva, and the image is uploaded to the cloud through the network. Compared with the prior art, the application does not need to transform the site, and realizes full-automatic fixed-point inspection. Since it is not necessary to pre-bury magnetic strips, tracks or densely deploy UWB / Bluetooth base stations in the stall, the problem of corrosion demagnetization of the magnetic strips in the humid manure environment and the multipath signal interference problem caused by the metal stall structure are avoided, and the early deployment cost and the later maintenance workload are significantly reduced.

Description

Technical Field

[0001] This invention relates to a sow farrowing house inspection robot, and more particularly to a fixed-point photography inspection robot for sows about to farrow based on indoor SLAM navigation, which is suitable for automated farming. Background Technology

[0002] Current inspections of sow farrowing pens are still primarily manual, resulting in high labor intensity and poor timeliness. Existing automation solutions mostly employ electromagnetic tracks or UWB / Bluetooth beacon navigation, requiring the pre-installation of magnetic strips, tracks, or dense deployment of base stations within the pens. This leads to high deployment costs, poor site adaptability, and issues such as corrosion and demagnetization of magnetic strips in damp, manure-contaminated environments, as well as multipath interference caused by the metal pen structure. LiDAR-based SLAM-based autonomous navigation technology requires no site modification, can create real-time maps, and autonomously plan global paths, making it the mainstream solution for indoor mobile robots. Applying it to sow farrowing pens to achieve fully automated, high-precision, timed, and fixed-point inspections represents a crucial technological direction in the field of livestock automation. Summary of the Invention

[0003] This invention addresses the problems of low efficiency and untimely inspection in manual inspection in the background technology, as well as the poor robustness of traditional electromagnetic navigation and the need for rewiring after pen renovation. It proposes a sow farrowing house inspection robot and its inspection method based on lidar SLAM positioning, which realizes fully automatic, high-precision, timed and fixed-point inspection without the need for site modification.

[0004] Technical solution:

[0005] This invention first discloses a sow farrowing house inspection robot, which includes an inspection chassis, a navigation control box, and an inspection camera unit, specifically:

[0006] The inspection chassis includes a motion motor, an electrical control box, an electrical control panel, an aluminum alloy shell, a chassis remote control receiver, a battery, and a speed encoder. The chassis remote control receiver is connected to the chassis motor control module inside the electrical control box via an SPI bus, and the speed encoder is connected to the chassis motor control module via an RS485 bus. The speed encoder is used to measure the motion motor speed. The motion motor and electrical control box are located inside the aluminum alloy shell of the inspection chassis. The electrical control panel is embedded in the outer surface of the inspection robot's aluminum alloy shell. The navigation control box and the inspection camera unit are connected to the upper surface of the inspection chassis.

[0007] - The navigation control box includes: an industrial computer, a fan for cooling the industrial computer, an operation display screen, a navigation radar, and a navigation camera; wherein: the navigation radar is connected to the industrial computer via a network cable, the industrial computer is connected to the operation display screen via an HDMI cable, and the navigation camera is located on the side of the robot's aluminum alloy shell and is connected to the industrial computer via a USB bus;

[0008] - The inspection camera unit includes: a lifting push rod, a top-mounted servo gimbal, and an inspection unit controller. The rotatable part of the servo gimbal houses a gyroscope and the inspection camera. The lifting height of the push rod is read by its built-in position encoder, and its speed is controlled by the input PWM of the motor driver. The inspection unit controller reads the encoder position feedback from the push rod and then uses a PID algorithm to control the PWM output, which in turn acts on the motor driver, quickly bringing the push rod to the predetermined height. The rotation angle of the servo gimbal is fed back by the gyroscope, and its rotation speed and position are controlled and read by the inspection unit controller. The rotation of the servo gimbal to the target shooting angle is controlled by a PID algorithm.

[0009] Preferably, the electrical control box is equipped with a motor driver, a step-down module, a junction box module, and a chassis motor control module. The positive and negative power lines of the lithium battery are connected to the junction box module, which is connected to the step-down module. The various voltages obtained by stepping down provide the required energy for the motor driver and the chassis motor control module. The motor driver requires 24V voltage, and the chassis motor control module requires 5V voltage.

[0010] The electrical control panel includes: an emergency stop switch, a main start switch, a chassis controller reset switch, a gimbal start switch, a charging port, a 12V output port, a 5V output port, a chassis mode selection button, a lithium battery, and a power monitoring meter. The emergency stop switch, main start switch, charging port, 12V output port, 5V output port, chassis mode selection button, lithium battery, and power monitoring meter are connected to the power supply board module. The emergency stop switch is used to stop the robot in special circumstances (when the robot causes damage to the environment or personnel). The main start switch is used to start the robot's power supply. The chassis mode selection button controls whether the robot is in navigation mode or remote control mode. The power monitoring meter monitors and displays the real-time voltage of the lithium battery. The 12V and 5V output ports are external interfaces for the internal step-down module.

[0011] Preferably, the input PWM control output of the motor driver for the lifting actuator is obtained by the following formula:

[0012]

[0013] Where u(k) is the control output of the lifting push rod at time k, which is used to drive the push rod to lift and lower; This is the proportionality coefficient. The integral coefficient is... These are the differential coefficients; , Let h(k) be the desired elevation height setting at time k, h(k) be the actual elevation height feedback value at time k, T be the sampling period, e(k-1) be the deviation at time k-1, and e(i) be the error calculated at the i-th sampling.

[0014] Preferably, when the inspection robot reaches the inspection target point, if the stopping angle deviates, the servo gimbal uses a PID algorithm to make the camera mounted on the servo gimbal aim at the sow's vulva according to the predetermined angle of the inspection camera relative to the positive direction of the sow's vulva.

[0015] The incremental PID control formula for rotating the servo gimbal to the target shooting angle position is as follows:

[0016]

[0017] Where Δu(k) is the control increment of the servo gimbal at the k-th sampling time; Kp is the proportional coefficient; Ki is the integral coefficient; Kd is the derivative coefficient; e(k) is the angle deviation at the k-th sampling time, i.e., the difference between the target shooting angle and the actual angle of the servo gimbal fed back by the gyroscope; e(k-1) is the angle deviation at the (k-1)-th sampling time; and e(k-2) is the angle deviation at the (k-2)-th sampling time.

[0018] The inspection unit controller obtains the current control quantity u(k) = u(k-1) + Δu(k) by superimposing the calculated control quantity increment Δu(k) onto the control quantity u(k-1) of the previous moment, and outputs it to control the rotation speed and position of the servo gimbal, thereby driving the servo gimbal to rotate to the target shooting angle position.

[0019] This invention also discloses an inspection method for a sow farrowing house inspection robot, which includes the following steps:

[0020] S1. Use LiDAR in the farrowing house of sows to construct a 3D point cloud map, obtain the 3D point cloud model of the pig house and convert it into a 2D raster map.

[0021] S2. Plan the inspection points in the sow farrowing house and obtain the inspection point YAML parameters of the robot's positioning information;

[0022] S3. Initiate the inspection of the sow farrowing house;

[0023] S4. Activate the inspection camera unit after arriving at the inspection point;

[0024] S5. Take photos of the sow's vulva area; after taking the photos, upload the sow's vulva images to the cloud server for storage via 4G network;

[0025] S6 returns to its initial position after completing the inspection within the set inspection time.

[0026] Preferably, S1 specifically includes:

[0027] S1-1. Use multi-line lidar to scan the environment of the pig house and farrowing room, use the FAST_lio algorithm to construct a 3D dense point cloud map of the sow factory, and save it as a Pcd file.

[0028] S1-2. Based on the height threshold segmentation method, the obtained 3D point cloud file of the sow farrowing house is segmented to obtain a clear point cloud pcd file of the wall, and a 2D raster map is generated at a resolution of 0.05m / pixel.

[0029] Preferably, a 2D raster map is obtained through the following steps:

[0030] Original point cloud for:

[0031]

[0032] in Points The three-dimensional coordinates The total number of point clouds, Represents the three-dimensional real number space;

[0033] Given height threshold Clouds dotting the wall Defined as:

[0034]

[0035] Raster coordinate transformation:

[0036] The grid resolution is r, and the map origin is... Pointing clouds at the wall Each point in Its corresponding raster coordinates Obtained by floor mapping:

[0037]

[0038] in, This indicates the floor function. Points Column and row pixel indices in a raster map;

[0039] Let the number of pixels in the u-column and v-row directions of the raster map be (U, V), then the map is... A 0-1 matrix; traversal For any point, if the raster cell (u, v) is mapped to by at least one point, it is set to 1; otherwise, it is set to 0; define the set of occupied raster cells. for:

[0040]

[0041] The elements of a raster map are sets. Characteristic function :

[0042]

[0043] The final output is a binary raster map. In the diagram, 1 indicates that the corresponding grid cell is occupied by the wall point cloud, and 0 indicates that it is not occupied.

[0044] Preferably, S2 specifically includes:

[0045] S2-1. Start the AMCL positioning algorithm and initialize the inspection robot's positioning;

[0046] S2-2. Subscribe to the robot's positioning information (x, y, yaw), where x represents the distance along the X-axis in the map coordinate system, y represents the distance along the Y-axis in the map coordinate system, and yaw represents the angle of rotation of the robot relative to the positive direction of the map origin in the map coordinate system. Use remote control to control the inspection robot to walk through the inspection route, move to the location to be inspected and record the positioning information, and save the inspection point as a YAML parameter file for subsequent automatic inspection.

[0047] Preferably, S3 specifically includes:

[0048] S3-1. Start the nav_map_server plugin service in the navigation framework, load the 2D raster map obtained in S1 into the navigation stack, and publish the map topic.

[0049] S3-2. Start the controller_server plugin service in the navigation framework and use the DWB algorithm to implement path following and obstacle avoidance.

[0050] S3-3. Start the AMCL positioning system to publish the TF transformation for robot positioning;

[0051] S3-4. Load the inspection point YAML parameter file obtained in S2, and load it into the inspection point stack in the order of saving the inspection points. Use the A* path planning algorithm to plan the path from the robot to the inspection point, and use the target inspector provided by the navigation framework to check whether the robot has reached the inspection point.

[0052] Preferably, S4 specifically includes:

[0053] S4-1. Activate the lifting push rod, which drives the servo motor gimbal to rise to the designated shooting height H;

[0054] S4-2, the servo motor gimbal drives the inspection camera to rotate 360 ​​degrees, and the shooting direction of the inspection camera is determined by the pre-set parameters.

[0055] Beneficial effects:

[0056] Compared with existing technologies, this invention constructs a 2D grid map of the pigsty based on LiDAR SLAM, and achieves autonomous navigation by combining AMCL positioning and A* path planning. It eliminates the need for pre-embedded magnetic strips, tracks, or dense deployment of UWB / Bluetooth base stations within the pens, thus avoiding the problems of magnetic strip corrosion and demagnetization in damp, manure-soiled environments and multipath signal interference caused by the metal pen structure. This significantly reduces initial deployment costs and subsequent maintenance workload, and no rewiring is required after pen layout adjustments. Upon reaching the inspection point, the lifting push rod precisely reaches the set shooting height under position-based PID control, and the servo gimbal incrementally... The automatic compensation for stopping angle deviation under the action of PID and gyroscope feedback ensures that the camera is always pointed in the direction of the sow's vulva, eliminating the shooting deviation caused by inconsistent viewing angles and heights during manual inspections, and ensuring the standardization and comparability of images from each inspection. The entire inspection process is executed automatically according to preset parameters, and the images are uploaded to the cloud server in real time after being captured, realizing 24-hour scheduled and fixed-point inspections and traceable management of image data. This greatly reduces the intensity of manual labor and the blind spots in inspection timeliness, making it easier for farmers to grasp the signs of sows about to give birth in a timely manner, and improving the efficiency of farrowing house management and reproductive survival rate. Attached Figure Description

[0057] Figure 1 This is a schematic diagram of the entire machine of the present invention.

[0058] Figure 2 This is a side view of the navigation control box structure of the present invention.

[0059] Figure 3 This is a front view schematic diagram of the overall structure of the present invention.

[0060] Figure 4 This is a top view of the overall structure of the present invention.

[0061] Figure 5 This is a diagram showing the internal electrical connections of the chassis used for inspection in this invention.

[0062] Figure 6 This is a schematic diagram of the electrical control components of the present invention.

[0063] Figure 7 This is a schematic diagram of the robot control panel of the present invention.

[0064] Figure 8 This is a cross-sectional view of the robot assembly of the present invention.

[0065] Figure 9 This is a flowchart of the robot's workflow in this invention. Detailed Implementation

[0066] The present invention will be further described below with reference to embodiments, but the scope of protection of the present invention is not limited thereto:

[0067] Example 1

[0068] Embodiment 1 of the present invention provides a robot for fixed-point inspection in sow farms, such as... Figure 1 As shown, the robot body consists of three parts: inspection chassis 100, navigation control box 200, and inspection camera unit 300.

[0069] The inspection chassis 100 is the motion carrier of the inspection robot. The navigation control box 200 and the inspection camera unit 300 are mounted on the inspection chassis 100. The navigation control box 200 is used for environmental perception and control of the movement of the chassis to realize the autonomous walking of the inspection chassis 100. The inspection control camera unit 300 is fixed in the central area of ​​the chassis and is used to take pictures of sows waiting to give birth at designated inspection locations.

[0070] Appendix Figure 2-6 The detailed structure of the inspection robot from different angles is given.

[0071] Further combine with the appendix Figure 7The electrical control panel 103 of the inspection chassis 100 is connected to the electrical control box 102. The emergency stop switch 110 controls the power supply to the motor driver 106 via the circuit board module 108, thereby controlling the start and stop of the chassis's motion motor 101. The main start switch 111 of the electrical control panel 103 controls the system power supply of the entire inspection robot. The chassis controller reset switch 112 on the electrical control panel 103 is connected to the reset circuit of the chassis motor control module 109 in the electrical control box 102 for resetting the chassis controller. The gimbal start switch 113 on the electrical control panel 103 controls the power supply to the inspection camera unit 300. The electrical control panel 103 also has a charging port. 114 is used to connect an external charger and charge the lithium battery 118. The 12V output port 115 and 5V output port 116 on the electrical control panel 103 are connected to the step-down module 107 in the electrical control box 102. The chassis mode selection button 117 on the electrical control panel 103 is used to check the switching of the chassis's movement mode. The chassis mode selection button 117 is controlled by the navigation control box 200 by default. In case of an emergency, the chassis remote control receiver 105 can receive the control signal sent by the remote control to remotely control the movement of the chassis. The power monitoring meter 119 on the electrical control panel 103 has its internal circuit connected in parallel to the positive and negative terminals of the motor to monitor the battery power information. When the power display is lower than 22.4V, it should be charged immediately.

[0072] Further combine with the appendix Figure 8 The robot assembly cross-section diagram shown shows that the industrial computer 201 in the navigation control box 200 is connected to the chassis motor control module 109 via a USB cable, transmitting motion control commands to control the movement of the inspection chassis 100; the navigation radar 204 is connected to the industrial computer 201 via a network cable to collect 3D point cloud data of the pigsty; the operation display screen 203 is connected to the industrial computer 201 via an HDMI cable to display the Ubuntu operating system interface on the industrial computer 201; and the cooling fan 202 is embedded in the aluminum alloy shell 104 to dissipate heat from the industrial computer.

[0073] The navigation camera 205 is located on the side of the robot housing and is connected to the industrial computer 201 via a USB bus;

[0074] The lifting push rod 301 and servo gimbal 302 in the inspection camera unit 300 can be controlled by the chassis motor control module 109. The lifting push rod 301 can move the inspection camera 304 in the Z-axis direction, and the servo gimbal 302 can rotate the inspection camera 304 360 degrees to take pictures. The gyroscope 303 is located on the top of the inspection camera. The chassis motor control module 109 reads the yaw angle of the gyroscope 303 through serial communication, and then controls the servo gimbal 302 to drive the inspection camera 304 to rotate so as to take pictures in different angles.

[0075] Furthermore, the lifting height of the lifting push rod 301 can be controlled by a position-based PID algorithm, the specific algorithm being:

[0076]

[0077] Where u(k) is the control output of the lifting push rod at time k (used to drive the push rod to lift and lower). This is the proportionality coefficient. The integral coefficient is... The differential coefficients are... ( Let h(k) be the expected rise / fall height setting value at time k, h(k) be the actual rise / fall height feedback value at time k, T be the sampling period, and e(k-1) be the deviation at time k-1.

[0078] Furthermore, the rotation angle of the servo gimbal 302 can be fed back by the gyroscope 303, and the rotation speed and position of the servo gimbal 302 can be controlled and read by the chassis motor control module 109. The rotation of the servo gimbal 302 to the target shooting angle position can be controlled by a PID algorithm; the incremental PID control formula for controlling the rotation of the servo gimbal 302 to the target shooting angle position is as follows:

[0079]

[0080] Where Δu(k) is the control increment of the servo gimbal at the k-th sampling time; Kp is the proportional coefficient; Ki is the integral coefficient; Kd is the derivative coefficient; e(k) is the angle deviation at the k-th sampling time, i.e., the difference between the target shooting angle and the actual angle of the servo gimbal fed back by the gyroscope 303; e(k-1) is the angle deviation at the (k-1)-th sampling time; and e(k-2) is the angle deviation at the (k-2)-th sampling time. The inspection unit controller superimposes the calculated control increment Δu(k) onto the control quantity u(k-1) at the previous moment to obtain the control quantity u(k) = u(k-1) + Δu(k) at the current moment, and outputs it to control the rotation speed and position of the servo gimbal 302, thereby driving the servo gimbal 302 to rotate to the target shooting angle position (the servo gimbal is initially reset and started at angle 0).

[0081] Example 2

[0082] Embodiment 2 of the present invention differs from Embodiment 1 in that it provides a navigation and mapping method for a sow farm inspection robot.

[0083] First, select remote control mode using chassis mode selection button 117 and start navigation radar 204. Then, use the fast_lio algorithm to build a 3D point cloud map. Control the inspection robot to move in the sow farm corridor using the remote control. Finally, save the 3D point cloud map to prepare for the next step of wall point cloud extraction.

[0084] Let the original point cloud be:

[0085]

[0086] in Points The three-dimensional coordinates of the point cloud are given, where N is the total number of points in the cloud.

[0087] Given height threshold (Unit: m), the wall point cloud is defined as:

[0088]

[0089] After extracting the wall point cloud, it needs to be converted into a 2D grid map for path planning of the inspection robot's navigation.

[0090] Raster coordinate transformation:

[0091] Let the raster resolution be r = 0.05, {m / pixel}, and the map origin be... Unit: m. For each point in the point cloud Q of the wall. Its corresponding raster coordinates Obtained by floor mapping:

[0092]

[0093] in This indicates the floor function. Points Column and row pixel indexes in a raster map.

[0094] Let the number of pixels in the u-column and v-row directions of the raster map be (U, V), then the map is... 0-1 matrix Iterate through all points in Q. If a grid cell (u, v) is mapped to by at least one point (i.e., occupied), set it to 1; otherwise, set it to 0. Define the set of occupied grid cells as:

[0095]

[0096] The elements of the raster map are characteristic functions of set C:

[0097]

[0098] In the final output binary raster map M, 1 indicates that the corresponding raster cell is occupied by the wall point cloud, and 0 indicates that it is not occupied.

[0099] Example 3

[0100] The third embodiment of the present invention differs from the second embodiment in that it provides a method for obtaining navigation points for inspection in the farrowing house of sows.

[0101] First, select remote control mode by pressing button 117 on the chassis mode selection panel, load the obtained grid map, then use the AMCL algorithm for positioning, start the robot and use the remote control to control the inspection robot to reach the designated inspection target point.

[0102] By using ROS2's topic subscription mechanism, the robot's localization information (x, y, yaw) published by the AMCL algorithm is obtained; all navigation target points are saved as YAML files.

[0103] Example of a YAML parameter file:

[0104] target_points: [

[0105] 2.0539, 0.4612, 30.450,

[0106] 4.4830, 0.3045, 30.4700,

[0107] 6.9120, 0.1477, 30.3200, ]

[0109] The saved Ymal parameter file is used for subsequent autonomous inspection and navigation. The robot navigates from the starting point to the predetermined inspection points in sequence, and returns to the starting point after completing the inspection task.

[0110] Example 4

[0111] Embodiment 4 of the present invention discloses a sow farrowing house inspection robot and its inspection method, combined with Figure 9 This includes the following steps, S1 to S6:

[0112] S1. Use LiDAR to construct a 3D point cloud map in the sow farrowing house, obtain a 3D point cloud model of the pigsty, and convert it into a 2D raster map:

[0113] S1-1. Use multi-line lidar to scan the environment of the pig house and farrowing room, and use the FAST_lio algorithm to construct a 3D dense point cloud map of the sow factory. It is best to save it as a Pcd file.

[0114] S1-2. Based on the height threshold segmentation method, the obtained 3D point cloud file of the sow farrowing house is segmented to obtain a clear point cloud pcd file of the wall, and a 2D raster map is generated at a resolution of 0.05m / pixel.

[0115] S2. Planning of inspection points in sow farrowing pens:

[0116] S2-1. Start the AMCL positioning algorithm and initialize the inspection robot positioning.

[0117] S2-2. Subscribe to the robot's location information (x, y, yaw), use remote control to control the inspection robot to follow the inspection route, move to the location to be inspected and record the location information, and save the inspection point as a yaml parameter file for subsequent automatic inspection.

[0118] S3. Initiate the sow farrowing house inspection task:

[0119] S3-1. Start the nav_map_server plugin service in the navigation framework, load the 2D raster map obtained in S1 into the navigation stack, and publish the map topic.

[0120] S3-2. Start the controller_server plugin service in the navigation framework and use the DWB algorithm to implement path following and obstacle avoidance.

[0121] S3-3. Start the AMCL positioning system to publish the TF transformation for robot positioning;

[0122] S3-4. Load the inspection point YAML parameter file obtained in S2, load it into the inspection point stack in the order of inspection point saving, use the A* path planning algorithm to plan the path from the robot to the inspection point, and use the target inspector provided by the navigation framework to check whether the robot has reached the inspection point.

[0123] S4. Upon arrival at the inspection point, activate the inspection camera unit:

[0124] S4-1. Start the lifting mechanism, which can drive the servo motor gimbal to raise the designated shooting height H.

[0125] S4-2, the pan-tilt head can drive the camera to rotate 360 ​​degrees, and the shooting direction of the camera can be determined by the pre-set parameters;

[0126] S5. Take photos of the sow's vulva area:

[0127] S5-1. After completing step S4, start the camera and take a picture of the sow's vulva area.

[0128] S5-2. After the shooting is completed, the image of the sow's vulva is uploaded to the cloud server for storage via 4G network.

[0129] S6. After completing the inspection within the set inspection time, return it to its initial position.

[0130] The specific embodiments described herein are merely illustrative of the spirit of the invention. Those skilled in the art to which this invention pertains may make various modifications or additions to the described specific embodiments or use similar methods to substitute them, without departing from the spirit of the invention or exceeding the scope defined by the appended claims.

Claims

1. A robot for inspecting sow farrowing pens, characterized in that... It includes an inspection chassis, a navigation control box, and an inspection camera unit, specifically: The inspection chassis includes a motion motor, an electrical control box, an electrical control panel, an aluminum alloy shell, a chassis remote control receiver, a battery, and a speed encoder. The chassis remote control receiver is connected to the chassis motor control module inside the electrical control box via an SPI bus, and the speed encoder is connected to the chassis motor control module via an RS485 bus. The speed encoder is used to measure the motion motor speed. The motion motor and electrical control box are located inside the aluminum alloy shell of the inspection chassis. The electrical control panel is embedded in the outer surface of the inspection robot's aluminum alloy shell. The navigation control box and the inspection camera unit are connected to the upper surface of the inspection chassis. - The navigation control box includes: an industrial computer, a fan for cooling the industrial computer, an operation display screen, a navigation radar, and a navigation camera; wherein: the navigation radar is connected to the industrial computer via a network cable, the industrial computer is connected to the operation display screen via an HDMI cable, and the navigation camera is located on the side of the robot's aluminum alloy shell and is connected to the industrial computer via a USB bus; - The inspection camera unit includes: a lifting push rod, a top-mounted servo gimbal, and an inspection unit controller. The rotatable part of the servo gimbal houses a gyroscope and the inspection camera. The lifting height of the push rod is read by its built-in position encoder, and its speed is controlled by the input PWM of the motor driver. The inspection unit controller reads the encoder position feedback from the push rod and then uses a PID algorithm to control the PWM output, which in turn acts on the motor driver, quickly bringing the push rod to the predetermined height. The rotation angle of the servo gimbal is fed back by the gyroscope, and its rotation speed and position are controlled and read by the inspection unit controller. The rotation of the servo gimbal to the target shooting angle is controlled by a PID algorithm.

2. The sow farrowing house inspection robot according to claim 1, characterized in that... The electrical control box contains a motor driver, a step-down module, a junction box module, and a chassis motor control module. The positive and negative power lines of the lithium battery are connected to the junction box module, which is connected to the step-down module. The various voltages obtained by stepping down provide the necessary energy for the motor driver and the chassis motor control module. The motor driver requires 24V, and the chassis motor control module requires 5V. The electrical control panel includes: an emergency stop switch, a main start switch, a chassis controller reset switch, a gimbal start switch, a charging port, a 12V output port, a 5V output port, a chassis mode selection button, a lithium battery, and a power monitoring meter. The emergency stop switch, main start switch, charging port, 12V output port, 5V output port, chassis mode selection button, lithium battery, and power monitoring meter are connected to the power supply module. The emergency stop switch is used to stop the robot in special circumstances; the main start switch is used to start the robot's power supply; the chassis mode selection button controls whether the robot is in navigation mode or remote control mode; the power monitoring meter monitors and displays the real-time voltage of the lithium battery; and the 12V and 5V output ports are external interfaces for the internal step-down module.

3. The sow farrowing house inspection robot according to claim 1, characterized in that... The input PWM control output of the motor driver for the lifting actuator is obtained by the following formula: Where u(k) is the control output of the lifting push rod at time k, which is used to drive the push rod to lift and lower; This is the proportionality coefficient. The integral coefficient is... These are the differential coefficients; , Let h(k) be the desired elevation height setting at time k, h(k) be the actual elevation height feedback value at time k, T be the sampling period, e(k-1) be the deviation at time k-1, and e(i) be the error calculated at the i-th sampling.

4. The sow farrowing house inspection robot according to claim 1, characterized in that... When the inspection robot reaches the inspection target point, if the stopping angle deviates, the servo gimbal will use a PID algorithm to make the camera mounted on the servo gimbal point towards the sow's vulva according to the predetermined angle of the inspection camera relative to the positive direction of the sow's vulva. The incremental PID control formula for rotating the servo gimbal to the target shooting angle position is as follows: Where Δu(k) is the control increment of the servo gimbal at the k-th sampling time; Kp is the proportional coefficient; Ki is the integral coefficient; Kd is the derivative coefficient; e(k) is the angle deviation at the k-th sampling time, i.e., the difference between the target shooting angle and the actual angle of the servo gimbal fed back by the gyroscope; e(k-1) is the angle deviation at the (k-1)-th sampling time; and e(k-2) is the angle deviation at the (k-2)-th sampling time. The inspection unit controller obtains the current control quantity u(k) = u(k-1) + Δu(k) by superimposing the calculated control quantity increment Δu(k) onto the control quantity u(k-1) of the previous moment, and outputs it to control the rotation speed and position of the servo gimbal, thereby driving the servo gimbal to rotate to the target shooting angle position.

5. An inspection method for a sow farrowing house inspection robot, characterized in that... It includes the following steps: S1. Use LiDAR in the farrowing house of sows to construct a 3D point cloud map, obtain the 3D point cloud model of the pig house and convert it into a 2D raster map. S2. Plan the inspection points in the sow farrowing house and obtain the inspection point YAML parameters of the robot's positioning information; S3. Initiate the inspection of the sow farrowing house; S4. Activate the inspection camera unit after arriving at the inspection point; S5. Take photos of the sow's vulva area; after taking the photos, upload the sow's vulva images to the cloud server for storage via 4G network; S6 returns to its initial position after completing the inspection within the set inspection time.

6. The inspection method according to claim 5, characterized in that... S1 specifically includes: S1-1. Use multi-line lidar to scan the environment of the sow farrowing house, use the FAST_lio algorithm to construct a 3D dense point cloud map of the sow factory, and save it as a Pcd file. S1-2. Based on the height threshold segmentation method, the obtained 3D point cloud file of the sow farrowing house is segmented to obtain a clear point cloud pcd file of the wall, and a 2D raster map is generated at a resolution of 0.05m / pixel.

7. The inspection method according to claim 6, characterized in that... A 2D raster map is obtained through the following steps: Original point cloud for: in Points The three-dimensional coordinates The total number of point clouds, Represents the three-dimensional real number space; Given height threshold Clouds dotting the wall Defined as: Raster coordinate transformation: The grid resolution is r, and the map origin is... Pointing clouds at the wall Each point in Its corresponding raster coordinates Obtained by floor mapping: in, This indicates the floor function. Points Column and row pixel indices in a raster map; Let the number of pixels in the u-column and v-row directions of the raster map be (U, V), then the map is... A 0-1 matrix; traversal For any point, if the raster cell (u, v) is mapped to by at least one point, it is set to 1; otherwise, it is set to 0; define the set of occupied raster cells. for: The elements of a raster map are sets. characteristic function : The final output is a binary raster map. In the diagram, 1 indicates that the corresponding grid cell is occupied by the wall point cloud, and 0 indicates that it is not occupied.

8. The inspection method according to claim 5, characterized in that S2 Specifically, it includes: S2-1. Start the AMCL positioning algorithm and initialize the inspection robot's positioning; S2-2. Subscribe to the robot's positioning information (x, y, yaw), where x represents the distance along the X-axis in the map coordinate system, y represents the distance along the Y-axis in the map coordinate system, and yaw represents the angle of rotation of the robot relative to the positive direction of the map origin in the map coordinate system. Use remote control to control the inspection robot to walk through the inspection route, move to the location to be inspected and record the positioning information, and save the inspection point as a YAML parameter file for subsequent automatic inspection.

9. The inspection method according to claim 5, characterized in that... S3 specifically includes: S3-1. Start the nav_map_server plugin service in the navigation framework, load the 2D raster map obtained in S1 into the navigation stack, and publish the map topic. S3-2. Start the controller_server plugin service in the navigation framework and use the DWB algorithm to implement path following and obstacle avoidance. S3-3. Start the AMCL positioning system to publish the TF transformation for robot positioning; S3-4. Load the inspection point YAML parameter file obtained in S2, and load it into the inspection point stack in the order of saving the inspection points. Use the A* path planning algorithm to plan the path from the robot to the inspection point, and use the target inspector provided by the navigation framework to check whether the robot has reached the inspection point.

10. The inspection method according to claim 5, characterized in that... S4 specifically includes: S4-1. Activate the lifting push rod, which drives the servo motor gimbal to rise to the designated shooting height H; S4-2, the servo motor gimbal drives the inspection camera to rotate 360 ​​degrees, and the shooting direction of the inspection camera is determined by the pre-set parameters.

Images