Automatic path planning and positioning method and device for livestock and poultry house inspection robot

Abstract

The present invention relates to the technical field of inspection robots, and discloses an automatic path planning and positioning method and device for an inspection robot in a livestock and poultry house. The automatic path planning and positioning method comprises: obtaining current environmental information of the livestock and poultry house, constructing a global map, and planning a global path for the inspection robot to walk on the global map; estimating and updating the real-time walking posture of the inspection robot based on the three-axis attitude angle and acceleration information and walking mileage information when the inspection robot walks, and controlling the inspection robot to walk along the global path; the present invention can quickly and accurately navigate and position the inspection robot when the walking path and road conditions of the livestock and poultry house are unclear, and the invention has low cost and is easy to maintain, and effectively makes up for the defects of traditional GPS navigation and other navigation methods that require advance planning of paths and laying of auxiliary equipment, and is suitable for the navigation and walking of the inspection robot in the breeding environment of livestock and poultry houses.

Description

Technical Field

[0001] The present invention relates to the technical field of inspection robots, and in particular to an automatic path planning and positioning method and device for an inspection robot for livestock and poultry houses. Background Art

[0002] In recent years, with the rapid development and popularization of the livestock and poultry industry, breeding companies have basically achieved the standardization and regularization of livestock and poultry breeding, as well as the automation and mechanization of breeding facilities. In livestock and poultry breeding, there are many structural forms of livestock and poultry houses. For example, for laying hen breeding, chicken houses are divided into stepped chicken houses or H-shaped chicken houses. These chicken houses have multi-row and multi-layer chicken cages. The cage length of the chicken cage is relatively long, and the cage layers are mostly more than 3 layers. For these complex-structured livestock and poultry houses, it is more necessary to configure intelligent, precise, and efficient inspection robots for automated inspection in livestock and poultry breeding.

[0003] The livestock and poultry house inspection robot is a mechanical device responsible for completing spatial mobile operations and is used to perform inspection operations in closed or semi-closed livestock and poultry houses. As for how to realize inspections in complex livestock and poultry house environments, path planning and positioning of the livestock and poultry house inspection robot are the key to solving this problem.

[0004] The following is a brief introduction to the common navigation and positioning technologies in the field of robot automatic walking control, as shown below:

[0005] (1) GPS navigation and positioning. Since the GPS locator will have weak or even no signal in the poultry house, the livestock and poultry house inspection robot cannot rely on GPS to achieve indoor positioning.

[0006] (2) Traditional magnetic navigation positioning is a relatively reliable navigation technology that achieves vehicle control and navigation by laying magnetic strips on the floor of livestock and poultry houses and measuring the magnetic field signals on the path during movement to obtain the position deviation between the vehicle itself and the target tracking path. However, when the magnetic navigation method is used for the inspection of livestock and poultry house inspection robots, a large number of electromagnetic tracks need to be laid manually, which is not only complex in construction, long in cycle, and high in maintenance cost, but also needs to be re-laid in the future facility upgrades and renovations, which is complex in operation and construction, prolongs the upgrade cycle, and thus reduces the production efficiency of livestock and poultry.

[0007] (3) Similar to the principle of GPS positioning, UWB technology, WIFI positioning technology, ultrasonic positioning technology, and Bluetooth indoor positioning technology for local space or indoor space positioning are relatively mature. These technologies transmit signals between multiple transmitters or multiple receivers, measure time difference, signal strength or pulse phase and other information that changes with distance, and then calculate the distance information to finally determine the position of the robot in space. However, whether it is indoor UWB positioning, ultrasonic positioning, WIFI positioning, Bluetooth positioning, or the magnetic navigation positioning shown above, they are highly dependent on the environment, and all need to be configured with auxiliary equipment or tags, and the movement path is often planned in advance. It can only be used in relatively reliable and stable environments with specific operating rules and order. It is subject to many restrictions, making it difficult to apply livestock and poultry inspection robots to walk and conduct inspections on unknown paths and rugged roads.

[0008] (4) In the positioning process, inertial dead reckoning and dead reckoning positioning methods are also used. They do not rely on external equipment or tags for positioning. They use the motor encoder or inertial measurement unit (IMU) carried by the robot itself. Through the feedback of the motor signal, it is assumed that the drive wheel achieves the corresponding displacement of the wheel according to the rotation angle of the motor, and then the accumulated position of the robot body is calculated. The latter obtains the motion state through the accelerometer and gyroscope, and estimates the position information through integral calculation. However, in practical applications, for inertial dead reckoning and dead reckoning positioning, although they do not rely on external equipment and tags, both methods have cumulative errors. They are very effective for short-time and short-distance position estimation, but in livestock and poultry houses, inspection robots need to patrol 24 hours a day. In the absence of external correction, the error is amplified after long-term accumulation. Therefore, they cannot be used alone in livestock and poultry house inspection robots to realize livestock and poultry house inspection operations. Summary of the invention

[0009] In order to overcome the above-mentioned problems or at least partially solve the above-mentioned problems, an embodiment of the present invention provides an automatic path planning and positioning method and device for a livestock and poultry house inspection robot, which is used to make up for the defects of traditional GPS navigation and other navigation methods that require advance planning of paths and laying of auxiliary equipment, and adapt to the walking of the inspection robot in the livestock and poultry house breeding environment.

[0010] In order to solve the above-mentioned technical problems, an embodiment of the present invention provides, on the one hand, an automatic path planning and positioning method for a livestock house inspection robot, comprising: S1, obtaining the current environmental information of the livestock house, constructing a global map, and planning the global path for the inspection robot to walk on the global map; S2, based on the three-axis attitude angle and acceleration information and walking mileage information of the inspection robot when walking, estimating and updating the real-time walking posture of the inspection robot, and controlling the inspection robot to walk along the global path.

[0011] Among them, it also includes: S3, performing local path planning on the global map, controlling the inspection robot to walk along the planned local path, and avoiding obstacles encountered by the inspection robot when walking along the global path.

[0012] Among them, S1 further includes: based on the initial state and target state of the inspection robot's walking, using the A-Star algorithm to automatically plan a shortest global path on the global map; S2 further includes: based on the real-time updated walking posture of the inspection robot, controlling the inspection robot to walk along the shortest global path.

[0013] Among them, the estimating and updating of the real-time walking posture of the inspection robot in S2 further includes: obtaining the predicted posture of the inspection robot when walking based on the three-axis attitude angle and acceleration information of the inspection robot when walking; using the predicted posture as the initial posture and scanning and matching it with the posture of the inspection robot on the global map; performing mutation detection to obtain the scanned matching posture; fusing the scanned matching posture with the predicted posture to obtain the estimated real-time walking posture of the inspection robot, and updating the real-time walking posture of the inspection robot.

[0014] Among them, S3 further includes: using the DWA algorithm to plan the local path of the inspection robot on the global map, and using the trajectory evaluation function shown in the following formula to determine the local path of the inspection robot:

[0015] G(v,w)=σ·(α·heading(v,w)+β·dist(v,w)+γ·velocity(v,w));

[0016] Among them, G(v,w) represents the trajectory evaluation function, heading(v,w) represents the angle between the inspection robot and the obstacle, dist(v,w) represents the distance between the inspection robot and the nearest obstacle that intersects with its walking trajectory, velocity(v,w) represents the walking speed of the inspection robot, α, β, γ are the weights of heading(v,w), dist(v,w) and velocity(v,w) respectively, and σ is the trajectory smoothing coefficient.

[0017] Among them, S2 further includes: when the inspection robot inspects livestock and poultry with abnormal physiological information while walking along the global path, the AMCL positioning algorithm is used on the global map to obtain the real-time position information of the inspection robot, and the position information is sent to the cloud server as the position information of livestock and poultry with abnormal physiological information.

[0018] Among them, S2 further includes: before the inspection robot walks along the global path, detecting the power level of the battery in the inspection robot; when the power level of the battery is lower than a preset threshold, controlling the inspection robot to walk to a preset charging position on the global map to find a charger and charge; after the battery is fully charged, controlling the inspection robot to walk along the global path.

[0019] Among them, S1 further includes: obtaining the current environmental information of the livestock house based on the laser radar, and building the global map based on the Gmapping algorithm; S2 further includes: obtaining the three-axis attitude angle and acceleration information of the inspection robot when walking based on the inertial measurement unit, and obtaining the walking mileage information of the inspection robot when walking based on the mileage measurement element.

[0020] Among them, on the other hand, an embodiment of the present invention provides a device based on the above-mentioned livestock house inspection robot automatic path planning and positioning method, including: a laser radar, an inertial measurement unit, a mileage measurement element and a control module; the laser radar, the inertial measurement unit and the mileage measurement element are respectively connected to the control module, and the control module is used to connect to the walking drive system of the inspection robot; wherein, the laser radar is used to collect the current environmental information of the livestock house, the inertial measurement unit is used to collect the three-axis attitude angle and acceleration information of the inspection robot when walking, the mileage measurement element is used to collect the walking mileage information of the inspection robot, and the control module is used to navigate and position the walking of the inspection robot through the walking drive system based on the information collected by the laser radar, the inertial measurement unit and the mileage measurement element.

[0021] Among them, it also includes: an ultrasonic sensor; the ultrasonic sensor is communicatively connected to the control module and is used to detect obstacles in front of the inspection robot.

[0022] The above one or more technical solutions in the embodiments of the present invention have at least one of the following technical effects:

[0023] The automatic path planning and positioning method and device for a livestock and poultry house inspection robot provided in an embodiment of the present invention do not need to be configured with auxiliary equipment or labels when navigating the inspection robot. When the walking path in the livestock and poultry house is not pre-planned, the inspection robot can obtain the current environmental information of the livestock and poultry house through the sensor equipment it carries, and can independently construct a global map and plan a global path based on the SLAM technology known in the art. Then, based on the three-axis attitude angle and acceleration information and walking mileage information of the inspection robot when walking, the real-time walking posture of the inspection robot is estimated and updated, and the inspection robot is controlled to walk along the global path to prevent its walking track from deviating.

[0024] In this process, by estimating and updating the real-time walking posture of the inspection robot, the errors occurring when the inspection robot navigates along the global path can be effectively eliminated. On the other hand, the navigation of the inspection robot based on the global path effectively eliminates the cumulative errors generated in the long-term estimation and updating of the walking posture of the inspection robot, thereby greatly improving the walking stability and positioning accuracy of the inspection robot on rough or sloping roads, and is suitable for the inspection operation of the inspection robot in livestock and poultry houses with frequent layout changes, thereby effectively making up for the defects of traditional GPS navigation and other navigation methods that require advance planning of paths and laying of auxiliary equipment. BRIEF DESCRIPTION OF THE DRAWINGS

[0025] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the following briefly introduces the drawings required for use in the embodiments or the description of the prior art. Obviously, the drawings described below are some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.

[0026] Figure 1 It is a flow chart of the automatic path planning and positioning method of the livestock and poultry house inspection robot shown in an embodiment of the present invention;

[0027] Figure 2 This is a flowchart of the specific navigation and positioning of the livestock and poultry house inspection robot shown in an embodiment of the present invention;

[0028] Figure 3It is a control structure block diagram of the device based on the automatic path planning and positioning method of the livestock and poultry house inspection robot shown in an embodiment of the present invention.

[0029] In the figure: 1. LiDAR; 2. Inertial measurement unit; 3. Odometer; 4. Control module; 5. Travel drive system; 6. Ultrasonic sensor; 7. Magnetometer. DETAILED DESCRIPTION

[0030] In order to make the purpose, technical solution and advantages of the embodiments of the present invention clearer, the technical solution in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0031] In the description of the present invention, it should be noted that, unless otherwise clearly specified and limited, the terms "installed", "connected", and "connected" should be understood in a broad sense, for example, it can be a fixed connection, a detachable connection, or an integral connection; it can be a mechanical connection or an electrical connection; it can be a direct connection, or it can be indirectly connected through an intermediate medium, or it can be the internal communication of two components. For ordinary technicians in this field, the specific meanings of the above terms in the present invention can be understood according to specific circumstances.

[0032] See also Figure 1 This embodiment provides an automatic path planning and positioning method for a livestock house inspection robot, including: S1, obtaining the current environmental information of the livestock house, building a global map, and planning the global path for the inspection robot to walk on the global map; S2, based on the three-axis attitude angle and acceleration information and walking mileage information of the inspection robot when walking, estimating and updating the real-time walking posture of the inspection robot, and controlling the inspection robot to walk along the global path.

[0033] Specifically, the automatic path planning and positioning method for the livestock and poultry house inspection robot shown in this embodiment does not need to configure auxiliary equipment or labels when navigating the inspection robot. When the walking path in the livestock and poultry house is not pre-planned, the inspection robot can use the sensor equipment it carries, such as: laser radar, to obtain the current environmental information of the livestock and poultry house, and can independently construct a global map and plan the global path based on the SLAM technology known in the art. Then, based on the three-axis attitude angle and acceleration information and walking mileage information of the inspection robot when walking, the real-time walking posture of the inspection robot is estimated and updated, and the inspection robot is controlled to walk along the global path to prevent its walking track from deviating.

[0034] In this process, by estimating and updating the real-time walking posture of the inspection robot, the errors occurring when the inspection robot navigates along the global path can be effectively eliminated. On the other hand, the navigation of the inspection robot based on the global path effectively eliminates the cumulative errors generated in the long-term estimation and updating of the walking posture of the inspection robot, thereby greatly improving the walking stability and positioning accuracy of the inspection robot on rough or sloping roads. It is suitable for the inspection operations of the inspection robot in the breeding environment of livestock and poultry houses, and thus effectively makes up for the defects of traditional GPS navigation and other navigation methods that require advance planning of paths and laying of auxiliary equipment.

[0035] It should be pointed out here that in this embodiment, the unit structure (such as a chicken cage) used for raising livestock and poultry in the livestock and poultry house is used as the target object for the inspection robot to pre-inspect. In the prior art, the navigation device that realizes map construction and automatic path planning based on the detection of the environment includes a radar navigation device, a visual navigation device, and a sonar navigation device. Considering the strong anti-interference ability of the laser radar, the laser radar can be used to obtain the current environmental information of the livestock and poultry house, which includes the distance, height, direction and other parameters of multiple targets in the livestock and poultry house; in order to facilitate data processing, the SLAM technology known in the art is used to autonomously construct a global map, wherein the SLAM technology refers to the real-time positioning and map construction technology, which is used for the inspection robot to create a map in a completely unknown environment under the condition that its own position is uncertain, and use the map for autonomous positioning and navigation.

[0036] At the same time, this embodiment uses an inertial measurement unit to obtain the three-axis attitude angle and acceleration information of the inspection robot when walking. The inertial measurement unit includes a three-axis accelerometer and a three-axis gyroscope known in the art. The three-axis accelerometer is used to output the acceleration information of the inspection robot in the three coordinate axis directions in its coordinate system, and the gyroscope is used to output the angular velocity information of the inspection robot in the three coordinate axis directions relative to the navigation coordinate system, so as to facilitate the calculation of its corresponding attitude based on the angular velocity and acceleration information of the inspection robot in three-dimensional space. Among them, the three-axis accelerometer based on the inertial measurement unit can obtain the angle of the inspection robot relative to its gravity direction, which can improve the performance of the inspection robot walking in rugged areas for SLAM, and the three-axis gyroscope based on the inertial measurement unit can obtain the walking angular velocity information of the inspection robot to improve the accuracy of the local positioning of the inspection robot based on SLAM.

[0037] Among them, in step S2, the real-time walking posture of the inspection robot can be estimated and updated based on the inertial measurement unit, specifically including: based on the three-axis attitude angle and acceleration information obtained by the inertial measurement unit, the walking posture of the inspection robot is predicted to obtain the predicted posture of the inspection robot when walking; the predicted posture is used as the initial posture and scanned and matched with the posture of the inspection robot on the global map; mutation detection is performed, and the filter and reference scanning data are updated according to the detection results to obtain the scan matching posture; the scan matching posture and the predicted posture are merged through the filter to obtain the estimated real-time walking posture of the inspection robot, and the real-time walking posture of the inspection robot can be updated according to the estimated walking posture.

[0038] In addition, this embodiment also uses an mileage measurement element to obtain the walking mileage information of the inspection robot. The mileage measurement element includes an encoder known in the art. The encoder is installed on a drive motor used to drive the inspection robot to walk forward, and calculates the mileage of the inspection robot along the planned path, so as to position the inspection robot on the planned map.

[0039] Preferably, this embodiment is based on the current environmental information of the livestock and poultry house. When creating the map, the Gmapping algorithm is used to construct a global map. The Gmapping algorithm is a commonly used open source SLAM algorithm based on the filtering SLAM framework, and the Gmapping algorithm is based on the RBpf particle filter algorithm, which separates the positioning and mapping processes, and performs positioning before mapping. It uses resampling adaptive technology, fully considers the particle dissipation problem and the characteristics of particles gradually updating weights and converging, and reduces the uncertainty of the position of the livestock and poultry house inspection robot in the particle filter, thereby greatly improving the inspection robot's acquisition of walking odometer information and the acquisition quality of information such as laser beam dot matrix based on lidar optimization acquisition.

[0040] At the same time, the Gmapping algorithm uses the joint probability distribution p(x 1：t ,m|z 1：t ,u 1：t-1 ) to estimate the map m and the position x of the inspection robot; where m represents the map, x 1:t =x1,…,x t , x 1:t represents the position and posture of the inspection robot, z 1:t =z1,…,z t , z 1:t Indicates the current environmental information of the livestock and poultry house obtained by the laser radar, u 1:t-1 =u1,…,u t-1 ,u 1:t-1Represents the walking mileage information of the inspection robot. Using the idea of ​​positioning first and then building a map, the above joint probability distribution can be further decomposed into the following formula:

[0041] p(x 1：t ,m|z 1：t ,u 1：t-1 )=p(m|x 1：t , z 1：t )·p(x 1：t |z 1：t ,u 1：t-1 ).

[0042] At the same time, after this embodiment creates a global map corresponding to the livestock house based on the Gmapping algorithm, in step S1, based on the initial state and target state of the inspection robot's walking, the A-Star algorithm is used to automatically plan a shortest global path on the global map; thus, in step S2, the inspection robot can be further controlled to walk along the shortest global path based on the walking posture of the inspection robot updated in real time.

[0043] Specifically, the A-Star algorithm is the most effective direct search method for solving the shortest path in a static road network, and is also an effective algorithm for solving many search problems. The closer the distance estimate in the A-Star algorithm is to the actual value, the faster the final search speed.

[0044] The formula of A-Star algorithm is expressed as: f(n) = g(n) + h(n);

[0045] Where f(n), g(n), and h(n) are all valuation functions. f(n) represents the estimated cost of the inspection robot from the initial state to the target state via state n in the state space. g(n) represents the actual cost of the inspection robot from the initial state to state n in the state space. h(n) represents the estimated cost of the best path for the inspection robot from state n to the target state.

[0046] For the above path search problem, the initial state, target state and state n correspond to the node position of the inspection robot in the global map, and the cost represents the distance. The selection of h(n) guarantees the condition of finding the shortest path (optimal solution), and the key lies in the selection of the evaluation function f(n), or the selection of h(n). Below, the evaluation function d(n) is used to express the distance from state n to the target state, and the selection of h(n) can be roughly divided into the following three cases:

[0047] (1) If h(n)<d(n), it means that the number of search points is large, the search range is large, the efficiency is low, but the optimal solution can be obtained.

[0048] (2) If h(n) = d(n), that is, the distance estimate h(n) is equal to the shortest distance, the search will be strictly carried out along the shortest path, and the search efficiency is the highest at this time.

[0049] (3) If h(n)>d(n), it means that the number of search points is small, the search range is small, and the efficiency is high, but it cannot guarantee the optimal solution.

[0050] Therefore, based on the A-Star algorithm, a shortest global path can be automatically planned on the global map to control the inspection robot to walk along the shortest global path.

[0051] When controlling the inspection robot to walk, the inspection robot is not omnidirectional, and its movement state is divided into forward and rotation. Therefore, the walking trajectory of the inspection robot within two adjacent moments Δt is an arc trajectory. The arc trajectory is approximated as a straight line trajectory. The inspection robot moves along the x-axis of the coordinate system at a speed v within two adjacent moments Δt. The distance is v·Δt. Based on the angle θ between the straight line trajectory and the x-axis of the world coordinate system, t , we only need to project this distance on the x-axis and y-axis of the world coordinate system, and we can get the displacements Δx and Δy of the inspection robot in the world coordinate system within two adjacent moments Δt. Thus, we can establish a motion model for the inspection robot as shown in the following formula:

[0052] Δx=νΔtcos(θ t );

[0053] Δy=νΔtsin(θ t ).

[0054] Furthermore, through the motion model established above, based on the walking speed (including forward speed and rotation speed) of the inspection robot, the walking trajectory of the inspection robot can be calculated. Therefore, multiple sets of speeds need to be used in this process to facilitate the calculation of the walking trajectory of the inspection robot. Here, for the inspection robot, its maximum and minimum walking speeds should be set.

[0055] Since the walking of the inspection robot is also affected by the performance of its own drive motor, that is, the torque of the drive motor is limited, there is a maximum acceleration and deceleration limit in the rotation. Therefore, there is a dynamic window in the cycle of the forward trajectory simulation of the inspection robot. The speed in this dynamic window is the speed that the inspection robot can actually reach, as shown in the following formula:

[0056] V d ={(v,w)|v∈[v c -v b Δt,v c +v a Δt],w∈[wc -w b Δt,w c +w a Δt]}; where V d is the speed set corresponding to the dynamic window, v and w are speed parameters, corresponding to the forward speed and rotation speed of the inspection robot, v c , w c are the current forward speed and rotation speed of the inspection robot, v a , w a are the maximum deceleration of the inspection robot forward and rotation, v b , w b are the maximum acceleration of the inspection robot forward and rotation respectively.

[0057] Preferably, in order to ensure that the inspection robot can effectively avoid obstacles on the path when walking along the global path, such as Figure 1 As shown, this embodiment also includes: S3, performing local path planning on the global map, controlling the inspection robot to walk along the planned local path, and avoiding obstacles encountered by the inspection robot when walking along the global path.

[0058] Specifically, this embodiment uses an ultrasonic sensor to detect whether there is an obstacle in front of the inspection robot. When an obstacle is detected in front of the inspection robot, the DWA algorithm is used to plan the local path of the inspection robot on the global map, and the trajectory evaluation function shown in the following formula is used to determine the local path of the inspection robot:

[0059] G(v,w)=σ·(α·heading(v,w)+β·dist(v,w)+γ·velocity(v,w)); wherein G(v,w) represents the trajectory evaluation function, heading(v,w) represents the angle between the inspection robot and the obstacle, dist(v,w) represents the distance between the inspection robot and the nearest obstacle that intersects with its walking trajectory, velocity(v,w) represents the walking speed of the inspection robot, α, β, γ are the weights of heading(v,w), dist(v,w) and velocity(v,w) respectively, and σ is the trajectory smoothing coefficient.

[0060] Specifically, the trajectory evaluation function can make the weights of the three parts α, β, and γ smoother through the trajectory smoothing coefficient σ, so that a certain distance is maintained between the planned local path and the obstacle. Based on the above trajectory evaluation function, the local path planned by the DWA algorithm is evaluated to obtain an optimal local path to control the inspection robot to walk along the local path, and return to the global path to walk after avoiding obstacles.

[0061] It should be pointed out here that when encountering an obstacle, based on the safety considerations of the inspection robot, in order to ensure that the inspection robot can stop before encountering the obstacle, the inspection robot is at the maximum deceleration V a Under these conditions, the constraints shown in the following formula should also be set:

[0062]

[0063] Figure 2 This is a flowchart of the specific navigation and positioning of the livestock and poultry house inspection robot shown in this embodiment. When navigating and positioning the inspection robot, manual control mode and automatic control mode can be selected. In the automatic control mode, the global map constructed by SLAM based on the laser radar shown in the above embodiment is imported into the control module of the inspection robot, and the mileage measurement element is started to count the mileage.

[0064] Next, based on the battery power detection chip known in the art, the power level of the battery in the inspection robot is detected. When the battery power is lower than a preset threshold, an alarm is activated to warn of charging, and the inspection robot is controlled to find a charger at a preset charging position on the global map. The charging control motor on the inspection robot drives the automatic charging module to combine with the charging part of the charging pile to start charging. After the battery is fully charged, the charging control motor drives the automatic charging module to retract, and based on the laser radar, the walking path information of the inspection robot is obtained, and the inspection robot is controlled to walk along the global path.

[0065] When the inspection robot encounters an obstacle while walking along the global path, local path planning is performed using the scheme shown in the above embodiment to avoid the obstacle; at the same time, when the inspection robot detects livestock and poultry with abnormal physiological information while walking along the global path, the AMCL positioning algorithm is used on the global map to obtain the real-time position information of the inspection robot, and the position information is sent to the cloud server as the position information of livestock and poultry with abnormal physiological information. When the inspection robot does not detect livestock and poultry with abnormal physiological information while walking along the global path, the inspection robot continues to maintain the current walking state.

[0066] Finally, according to the above-mentioned walking planning method for the inspection robot, the inspection robot is controlled to continue walking along the global path until the inspection of the livestock and poultry house is completed.

[0067] Preferably, if Figure 3As shown, the present embodiment also provides a device based on the above-mentioned method for automatic path planning and positioning of the livestock house inspection robot, comprising: a laser radar 1, an inertial measurement unit 2, a mileage measurement element 3 and a control module 4; the laser radar 1, the inertial measurement unit 2 and the mileage measurement element 3 are respectively connected to the control module 4, and the control module 4 is used to connect to the walking drive system 5 of the inspection robot; wherein, the laser radar 1 is used to collect the current environmental information of the livestock house, the inertial measurement unit 2 is used to collect the three-axis attitude angle and acceleration information of the inspection robot when walking, the mileage measurement element 3 is used to collect the walking mileage information of the inspection robot, and the control module 4 is used to navigate and position the walking of the inspection robot through the walking drive system 5 based on the information collected by the laser radar 1, the inertial measurement unit 2 and the mileage measurement element 3.

[0068] Specifically, this embodiment sets up a laser radar 1, an inertial measurement unit 2, and an mileage measurement element 3 respectively connected to a control module 4, and connects the control module 4 to a walking drive system 5 of the inspection robot. When controlling the inspection robot to walk, the laser radar 1 obtains the current environmental information of the livestock house, and the control module 4 can automatically perform map construction and path planning based on the SLAM technology known in the art; at the same time, the three-axis attitude angle and acceleration information and walking mileage information of the inspection robot when walking along the planned path can be obtained through the inertial measurement unit 2 and the mileage measurement element 3, and the real-time position information and attitude information of the inspection robot can be obtained, thereby realizing navigation and positioning of the walking inspection robot.

[0069] It can be seen that the present invention does not need to configure auxiliary equipment or labels when navigating the inspection robot. When the walking path in the livestock house is not pre-planned, the inspection robot can be quickly navigated and positioned in the livestock house through the sensor equipment carried by the inspection robot itself, and the walking posture of the inspection robot under rugged road conditions can be better controlled to accurately position the inspection robot. It is low-cost and easy to maintain, and effectively makes up for the defects of traditional GPS navigation and other navigation methods that require advance planning of paths and laying of auxiliary equipment, and is suitable for the walking of inspection robots in livestock houses with frequent layout changes.

[0070] It should be pointed out here that the current environmental information of the livestock and poultry house collected by the laser radar 1 includes parameters such as the distance, height, and direction of multiple targets in the livestock and poultry house; the laser radar can be a TOF laser radar known in the art, and the maximum detection distance of the TOF laser radar is greater than 10m.

[0071] The control module 4 can adopt a PLC controller known in the art, and the walking drive system 5 includes a walking drive circuit known in the art and a drive motor adapted to each walking wheel of the inspection robot, so that the control module 4 is connected to the walking drive circuit, and the walking drive circuit is connected to the corresponding drive motors of each walking wheel on the inspection robot to realize the control of the walking posture of the inspection robot.

[0072] Preferably, the present embodiment further includes: an ultrasonic sensor 6; the ultrasonic sensor 6 is communicatively connected to the control module 4, and is used to detect obstacles in front of the inspection robot.

[0073] Specifically, the ultrasonic sensor 6 includes a transmitting probe and a receiving probe, which are used to transmit ultrasonic waves and receive echoes reflected by the ultrasonic waves when the transmitted ultrasonic waves touch an obstacle. Therefore, when the ultrasonic sensor 6 detects an obstacle within a preset distance in front of the inspection robot, it can immediately feedback a signal to the control module 4, so that the control module 4 can control the inspection robot to stop walking through the walking drive system 5, and perform local path planning for the walking of the inspection robot, thereby preventing direct collisions with livestock houses and other obstacles.

[0074] Finally, it should be pointed out that when navigating the inspection robot, a magnetometer 7 can also be configured. The magnetometer 7 is also called a magnetometer or a Gaussmeter in the art. It is used to detect the magnetic field of the current environment of the livestock house. It can provide the inspection robot with the east, south, west, and north orientation in the real world, thereby improving the efficiency of the inspection robot's repositioning, and can cooperate with the inertial measurement unit 2 to further improve the accuracy of the inspection robot's navigation and positioning.

[0075] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, improvements, etc. made within the spirit and principle of the present invention should be included in the protection scope of the present invention.

Claims

1. A method for automatic path planning and positioning of a livestock and poultry house inspection robot, characterized in that: include: S1, obtaining the current environmental information of the livestock and poultry house, constructing a global map, and planning a global path for the inspection robot to walk on the global map; S2, based on the three-axis attitude angle and acceleration information and walking mileage information of the inspection robot when walking, estimate and update the real-time walking posture of the inspection robot, and control the inspection robot to walk along the global path; S1 further includes: based on the initial state and target state of the inspection robot walking, automatically planning a shortest global path on the global map using the A-Star algorithm; S2 further includes: based on the walking posture of the inspection robot updated in real time, controlling the inspection robot to walk along the shortest global path; The estimating and updating of the real-time walking posture of the inspection robot described in S2 further includes: obtaining the predicted posture of the inspection robot when walking based on the three-axis attitude angle and acceleration information of the inspection robot when walking; using the predicted posture as the initial posture to scan and match the posture of the inspection robot on the global map; performing mutation detection to obtain the scan matching posture; fusing the scan matching posture with the predicted posture to obtain the estimated real-time walking posture of the inspection robot, and updating the real-time walking posture of the inspection robot.

2. The automatic path planning and positioning method for livestock and poultry house inspection robots according to claim 1 is characterized in that: Also includes: S3, performing local path planning on the global map, controlling the inspection robot to walk along the planned local path, and avoiding obstacles encountered by the inspection robot when walking along the global path.

3. The automatic path planning and positioning method for livestock and poultry house inspection robots according to claim 2 is characterized in that: S3 further includes: using the DWA algorithm to plan the local path for the inspection robot to walk on the global map, and using the trajectory evaluation function shown in the following formula to determine the local path for the inspection robot to walk: G(v,w)=σ·(α·heading(v,w)+β·dist(v,w)+γ·velocity(v,w)); Among them, G(v,w) represents the trajectory evaluation function, heading(v,w) represents the angle between the inspection robot and the obstacle, dist(v,w) represents the distance between the inspection robot and the nearest obstacle that intersects with its walking trajectory, velocity(v,w) represents the walking speed of the inspection robot, α, β, γ are the weights of heading(v,w), dist(v,w) and velocity(v,w) respectively, and σ is the trajectory smoothing coefficient.

4. The automatic path planning and positioning method for livestock and poultry house inspection robots according to claim 1, characterized in that: S2 further includes: when the inspection robot inspects livestock and poultry with abnormal physiological information while walking along the global path, the AMCL positioning algorithm is used on the global map to obtain the real-time position information of the inspection robot, and the position information is sent to the cloud server as the position information of the livestock and poultry with abnormal physiological information.

5. The automatic path planning and positioning method for livestock and poultry house inspection robots according to claim 1, characterized in that: S2 further includes: before the inspection robot walks along the global path, detecting the power level of the battery in the inspection robot; when the power level of the battery is lower than a preset threshold, controlling the inspection robot to walk to a preset charging position on the global map to find a charger and charge; after the battery is fully charged, controlling the inspection robot to walk along the global path.

6. The automatic path planning and positioning method for livestock and poultry house inspection robots according to claim 1, characterized in that: S1 further includes: obtaining current environmental information of the livestock and poultry house based on the laser radar, and constructing the global map based on the Gmapping algorithm; S2 further includes: obtaining the three-axis attitude angle and acceleration information of the inspection robot when walking based on the inertial measurement unit, and obtaining the walking mileage information of the inspection robot when walking based on the mileage measurement element.

7. A device based on the automatic path planning and positioning method of a livestock and poultry house inspection robot according to any one of claims 1 to 6, characterized in that: include: A laser radar, an inertial measurement unit, an mileage measurement element and a control module; the laser radar, the inertial measurement unit and the mileage measurement element are respectively connected to the control module, and the control module is used to connect to the walking drive system of the inspection robot; Among them, the laser radar is used to collect the current environmental information of the livestock and poultry house, the inertial measurement unit is used to collect the three-axis attitude angle and acceleration information of the inspection robot when walking, the mileage measurement element is used to collect the walking mileage information of the inspection robot, and the control module is used to navigate and position the walking of the inspection robot through the walking drive system based on the information collected by the laser radar, the inertial measurement unit and the mileage measurement element.

8. The device according to claim 7, characterized in that It also includes: an ultrasonic sensor; the ultrasonic sensor is communicatively connected to the control module and is used to detect obstacles in front of the inspection robot.

Images