A method and system for path planning of a cage poultry house autonomous inspection robot

Abstract

This invention belongs to the field of path planning technology for inspection robots, and discloses a path planning method and system for an autonomous inspection robot in caged poultry houses. First, a hierarchical cost map is established at the global layer, including a static map layer, a dynamic obstacle layer, an expansion layer, and a risk field layer. Orthogonal cumulative occupancy statistics are used to distinguish between the inter-cage passage area and the turning area, and the inspection requirement of "driving along the centerline of the passage" is explicitly encoded into the risk field. During the search phase, a multi-dimensional constrained adaptive A* algorithm is used to jointly model the cumulative cost, dynamic weighted heuristic, and risk cost, and collinear redundant nodes are removed after obtaining the initial path. At the local layer, key nodes are used as local targets. A 2.5D velocity constraint with lifting stability coupling, an adaptive sampling resolution based on environmental congestion, and a multi-vertex Jacobian constraint angular velocity correction for dynamic obstacles are introduced, extending the constant curvature trajectory of the traditional DWA to a variable curvature trajectory, achieving efficient obstacle avoidance for dynamic obstacles.

Description

Technical Field

[0001] This invention belongs to the field of poultry farming automation and inspection robot path planning technology, and specifically relates to a path planning method and system for an autonomous inspection robot in caged poultry houses. Background Technology

[0002] With the deepening of agricultural modernization in my country, the poultry farming industry is rapidly transforming towards intensification and intelligentization. Due to the low efficiency and high risk of disease transmission associated with traditional manual inspection methods, the use of autonomous inspection robots to replace manual labor has become an urgent demand for modern animal husbandry. Autonomous inspection robots for caged poultry houses are automated equipment integrating environmental perception, autonomous positioning, and navigation control. They can replace manual labor in tasks such as environmental information collection, identification of sick and dead chickens, and equipment status monitoring. Designing a path planning method that balances global optimization and local obstacle avoidance is fundamental to ensuring that the robot can perform autonomous inspections efficiently and safely in caged poultry house environments.

[0003] In existing path planning methods, Algorithms and the Dynamic Window Approach (DWA) are the most widely used global and local programming methods. However, for inspection tasks in the special environment of caged poultry houses, existing technologies have the following shortcomings: At the global programming level, traditional... The algorithm's eight-neighborhood node expansion method and heuristic function calculation do not consider the special environment of poultry houses, resulting in a large number of redundant nodes in the global path, reducing computational efficiency, and failing to meet the operational requirement that the robot needs to travel along the centerline of the cage passage. At the local planning level, traditional DWA uses fixed-velocity sampling, which is difficult to balance computational load and obstacle avoidance accuracy in environments like poultry houses where the degree of local congestion varies greatly. Traditional DWA is too passive in handling dynamic obstacles, generating mostly constant curvature trajectories, resulting in limited obstacle avoidance flexibility.

[0004] The statements in this section are merely background information related to the present invention and do not necessarily constitute prior art. Summary of the Invention

[0005] To address the problems of redundancy in global planning nodes, easy edge-hugging driving, and lag in return after obstacle avoidance in local planning in traditional poultry house environments, this invention proposes a path planning method for an autonomous inspection robot in caged poultry houses, so as to achieve effective global path planning and flexible local obstacle avoidance in poultry house scenarios.

[0006] To achieve the above objectives, the present invention adopts the following technical solution: A path planning method for an autonomous inspection robot in a caged poultry house includes the following steps: Step 1. Construct a two-dimensional grid map, generate a layered cost map, and initialize the inspection task information; Step 2. Adaptive use of multidimensional constraints The algorithm plans the initial global path on a hierarchical cost map; Step 3. By using a redundant node removal mechanism, collinear redundant points in the initial global path are eliminated, and only key nodes are retained to generate a globally optimal path node sequence with better geometric features; Step 4. Select key nodes in the global optimal path node sequence one by one as local navigation targets, and use the improved dynamic window method to select the local optimal trajectory to control the robot's movement and obstacle avoidance until the destination is reached.

[0007] Furthermore, based on the aforementioned path planning method for autonomous inspection robots in caged poultry houses, this invention also proposes a corresponding path planning system for autonomous inspection robots in caged poultry houses, the technical solution of which is as follows: A path planning system for an autonomous inspection robot in a caged poultry house includes the following modules: The map building module is used to build a two-dimensional raster map, generate a layered cost map, and initialize inspection task information; An initial global path building block for adaptive use with multidimensional constraints. The algorithm plans the initial global path on a hierarchical cost map; The global optimal path generation module is used to remove collinear redundant points in the initial global path through a redundant node removal mechanism, retaining only key nodes, in order to generate a global optimal path node sequence with better geometric features. The system also includes a navigation control module, which selects key nodes from the global optimal path node sequence one by one as local navigation targets, uses an improved dynamic window method to optimize the local optimal trajectory, and controls the robot's movement and obstacle avoidance until it reaches the destination.

[0008] Furthermore, based on the aforementioned path planning method for the autonomous inspection robot in caged poultry houses, this invention also proposes a computer device, which includes a memory and one or more processors. Executable code is stored in the memory. When the processor executes the executable code, it implements the steps of the aforementioned path planning method for the autonomous inspection robot in caged poultry houses.

[0009] Furthermore, based on the aforementioned path planning method for the autonomous inspection robot of caged poultry houses, this invention also proposes a computer-readable storage medium storing a program that, when executed by a processor, implements the steps of the aforementioned path planning method for the autonomous inspection robot of caged poultry houses.

[0010] The present invention has the following advantages: As described above, this invention relates to a path planning method for an autonomous inspection robot in caged poultry houses. This method reconstructs the inspection scenario for caged poultry houses by first establishing a hierarchical cost map at the global level, including a static obstacle layer, a dynamic obstacle layer, an expansion layer, and a risk field layer. Orthogonal cumulative occupancy statistics are used to automatically distinguish between cage passageways and turning areas. The inspection requirement of "traveling along the centerline of the passageway" is then explicitly encoded into the risk field. Subsequently, multi-dimensional constraint adaptive optimization is employed during the search phase. The algorithm jointly models cumulative cost, dynamically weighted heuristics, and risk cost. After obtaining the initial path, it further removes collinear redundant nodes and retains key nodes. At the local layer, it takes the global key nodes as local targets and introduces 2.5D velocity constraints with lifting stability coupling, adaptive sampling resolution based on environmental congestion, and multi-vertex Jacobian constraint angular velocity correction for dynamic obstacles. This extends the constant curvature trajectory of traditional DWA to a variable curvature trajectory, achieving efficient obstacle avoidance for dynamic obstacles. This invention effectively solves the problem that inspection robots in caged poultry houses struggle to achieve both full coverage and point-to-point inspection. Even with unknown obstacles, it enables safe and efficient autonomous navigation and inspection operations. Attached Figure Description

[0011] Figure 1 This is a flowchart of the path planning method for the autonomous inspection robot of caged poultry houses in an embodiment of the present invention; Figure 2 This is a schematic diagram of the layered cost map architecture in an embodiment of the present invention; Figure 3 This is a schematic diagram of the global path in an embodiment of the present invention; Figure 4 This is a diagram showing the mapping relationship between the vertices and the chassis in an embodiment of the present invention; Figure 5 This is a schematic diagram of the actual path in an embodiment of the present invention. Detailed Implementation

[0012] The present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments: Example 1 like Figure 1 As shown, the path planning method for the autonomous inspection robot in caged poultry houses in this embodiment includes the following steps: Step 1. Construct a two-dimensional grid map, generate a layered cost map, and initialize the inspection task information.

[0013] This invention mainly uses the static structural information of the caged poultry house environment and the environmental information acquired by the robot in real time, combined with the functional characteristics of different work areas, to assign differentiated passage costs and risk constraints to different grids in the environment, construct a hierarchical cost map for path planning, and initialize inspection task-related information, thereby providing a unified environmental modeling foundation for subsequent initial global path search, key node extraction and local obstacle avoidance control.

[0014] like Figure 2 As shown, the two-dimensional grid environment map in this embodiment adopts a layered cost map architecture.

[0015] In caged poultry housing environments, global path planning must meet the requirements of path safety and operational standardization. Traditional The algorithm only performs node expansion search on a single grid map and lacks a hierarchical expression of safety buffers and behavioral constraints in the workspace. Therefore, this invention adopts a hierarchical cost map architecture, which encodes environmental information from different sources and at different scales into independent cost layers and merges them according to predefined hierarchical rules to construct a global cost map that can be directly called by the path planner, providing a cost basis for subsequent node search.

[0016] The layered cost map comprises four independent data layers: a static map layer, a dynamic obstacle layer, an inflation layer, and a risk field layer. The formation methods for each data layer are as follows: Both the static obstacle layer and the dynamic obstacle layer are generated based on two-dimensional occupancy raster maps constructed using Simultaneous Localization and Mapping (SLAM).

[0017] The static map layer is a two-dimensional occupancy grid reference map pre-generated based on the known structural environment of the caged poultry house and the prior environmental information of the fixed cage rack layout, through scale calibration, coordinate mapping and raster modeling.

[0018] The dynamic obstacle layer is used to detect temporary obstacles in the environment based on the environmental information perceived in real time by the robot's sensors, and to mark the grid corresponding to the detected temporary obstacles as occupied, so as to realize the real-time update of the cost map.

[0019] In this embodiment, the sensors include a lidar sensor, an odometer, and an attitude sensor, used to collect real-time two-dimensional environmental data of the caged poultry house and the robot's current pose information. The lidar sensor is used to detect obstacles in real time, while the odometer and attitude sensor are used to obtain the robot's real-time motion state and heading angle.

[0020] The dynamic obstacle layer enables robots to respond promptly to dynamic changes in the environment during local planning and obstacle avoidance.

[0021] In addition, an expansion layer is set up to ensure that the overall plan meets the safety distance requirements between the plan and obstacles.

[0022] In this embodiment, the obstacle boundaries in the static map layer and the dynamic obstacle layer are used as the basis, and a safe width is established outward along the obstacle boundaries. A buffer zone is provided to prevent the robot from running close to the edge.

[0023] Safe width The calculation formula is: .in, The safety expansion coefficient is used to characterize the proportional relationship of the outward expansion of the obstacle boundary, and In this embodiment, For example, the value can be 2.

[0024] The equivalent circle radius of the robot satisfy . For vehicle body length, This refers to the width of the vehicle body.

[0025] Let the occupancy status of each grid cell in the layered cost map be... ,in ,when When, it indicates that the grid cell is an obstacle grid. When the value is 0, it indicates that the grid cell is an empty grid cell.

[0026] For each obstacle grid, the distance from its boundary shall not exceed The grid markings indicate impassable areas, forming an expansion layer.

[0027] In addition, to ensure the quality of robot inspection tasks, a risk field layer needs to be established in the cost map.

[0028] Because the working environment of caged poultry houses has obvious functional area differences, namely: Within the cage aisles, the robot travels along the centerline of the aisle to simultaneously acquire clear image information of the interior of the cages on both sides; in other areas, the robot needs to quickly turn around and enter the next aisle to reduce inspection time and energy consumption.

[0029] Therefore, a risk field layer is established in the cost map, and differentiated constraints are assigned to different areas to ensure that the planned path simultaneously meets the requirements of inspection operation quality and traffic efficiency. The specific process is as follows: Based on the inspection task requirements, the robot's operating area is divided into cage passage areas. With the transfer area .

[0030] The cage passage area This is the main travel area for the robot to perform inspection operations along the aisles between the cages; the turning area. This is the robot's non-operational area, where no path centering constraint is applied to the path.

[0031] This invention proposes an automatic method for distinguishing between cage passage areas. Other regions An automatic classification method for areas based on orthogonal cumulative occupancy statistics is used to automatically distinguish between cage passage areas. With the transfer area The process is as follows:

[0032] Let the occupancy status of each raster cell in the static map layer be denoted as . ,in And set the map width of the static map layer to be , length is .

[0033] Calculating horizontal edge functions based on static map layers : .

[0034] in Indicates the first static map layer The number of grid cells in a row that are occupied by obstacles.

[0035] In conclusion After that, it will satisfy The row is used as a candidate job row.

[0036] This represents the row-direction obstacle threshold, used to indicate the ratio between the number of grid cells occupied by obstacles in the y-th row and the total number of horizontal grid cells. .

[0037] Sort all candidate job rows in ascending order of row index. If the row index difference between two adjacent candidate job rows is 1, they are grouped into the same continuous row interval; otherwise, a new continuous row interval is opened, and finally, candidate continuous row intervals are obtained.

[0038] Let the starting row index of the candidate continuous row interval be denoted as The terminating row index is denoted as Therefore, the interval express The upper and lower boundaries of the region are determined, and then the statistical function of the vertical edge is calculated. as follows: .

[0039] in, Indicates the x-th column in the static map layer The number of grid cells occupied by obstacles. The vertical span of work area T1 is... Based on this, a candidate column set is constructed. as follows: .

[0040] in The threshold for obstacle in the column direction, and .

[0041] Candidate column set After sorting by x in ascending order, interval clustering is performed based on the continuity of column indexes. If the row index difference between two adjacent candidate columns is 1, they are assigned to the same continuous column interval.

[0042] Otherwise, initialize a new column interval, thereby obtaining the inter-cage aisle column interval. .

[0043] in, , They represent the first The starting and ending positions of the transverse section of the passageway between the bar cages.

[0044] Based on the transverse intervals of the cage aisles and the work area The upper and lower boundaries automatically divide the map grid into cage passage areas. With the transfer area ,in: ; ; in This represents the number of cage passageways identified. Grids that meet the above conditions are respectively assigned to the cage passageway area. and transfer area The risk level is in the cage passageway area. Using the centerline of the cage passage as a reference, a risk potential field is constructed that increases non-linearly from the inside out according to the lateral distance from the grid nodes to the centerline; in the turning area Inside, the risk value is set to zero.

[0045] For any grid node Its risk potential energy is defined as : .

[0046] in This is the risk amplitude coefficient. , , .

[0047] Pre-calculation of the inter-cage passage area The risk value of each grid cell within the region is calculated, and a risk field layer matrix with the same resolution and size as the global grid map is formed. , This is the lower limit of the channel half-width, typically taken as 0.5 grid units. The risk cost of grid nodes during node expansion is determined by... Read from the table.

[0048] In the cage passage area Within the grid, as the grid nodes gradually approach the channel boundary, their lateral distance to the channel centerline increases, and the corresponding risk value also increases, thereby driving the planned path away from the cage and towards the channel centerline.

[0049] In the transfer area Inside, the risk cost is set to zero, so that path planning is mainly constrained by the shortest path principle, ensuring that the robot can turn around at a lower cost.

[0050] To avoid repeatedly calculating the risk assessment value for each extended node during the global path search process, based on the above definition, the risk assessment value is pre-calculated. The risk value of each grid cell within the region is calculated, and a risk field layer matrix with the same resolution and size as the global grid map is formed. .

[0051] During node expansion, the risk cost of grid nodes is no longer obtained through real-time calculation, but is directly derived from the risk field layer matrix. Use table lookup to reduce search overhead and improve planning efficiency.

[0052] Step 2. Adaptive use of multidimensional constraints The algorithm plans the initial global path on a hierarchical cost map.

[0053] Heuristic function The settings directly affect The search performance of algorithms faces certain limitations when using traditional heuristic functions in intensive poultry house environments. Firstly, robots are confined to the passageways between cages and cannot traverse the facility in a straight line. Using Euclidean distance as a heuristic function may underestimate the actual distance cost, and since robots need to turn, Manhattan distance often overestimates the distance cost. Secondly, fixed-weight evaluation mechanisms cannot dynamically balance search speed and accuracy based on the search progress, lacking adaptability. Therefore, this invention proposes a distance-aware, dynamically weighted heuristic strategy, as follows: Adaptive The algorithm uses nodes in the raster map as path search objects, establishes Open and Closed tables, and traverses and filters the neighboring nodes of the current node; for neighboring nodes that meet the passability conditions, its path cost is calculated.

[0054] During the path search process, for any candidate node Construct a comprehensive evaluation function : .

[0055] in Indicates the distance from the starting point to the node. The cumulative actual cost, Indicates from node Heuristic cost estimation to the target node The dynamic weight coefficients for the heuristic term.

[0056] Represents a node The risk value in the risk field layer. According to the node The corresponding raster coordinates are derived from the pre-constructed risk field layer matrix. Obtained from the middle.

[0057] Heuristic function Distance estimation based on diagonal movement is used, and its calculation formula is as follows: ; ;in The cost of a robot moving one unit distance. The coordinates of the target node. These are the coordinates of the current node.

[0058] Dynamic weighting coefficients The calculation method is as follows: The system adaptively adjusts based on the search progress. ; In the initial stage of the search, the dynamic weight coefficient To speed up the search, a larger value is chosen, and the dynamic weight coefficient is adjusted as the search process approaches the target node. Gradually decrease the path size to improve path planning accuracy.

[0059] Adaptive The algorithm selects from the Open table during the iteration process. The smallest node is selected as the current expansion node, and its neighboring nodes are calculated and updated accordingly. , and It also maintains the Open and Closed tables.

[0060] When the target node is selected or the preset termination condition is met, the initial global path is obtained by backtracking based on the parent node pointer.

[0061] In summary, this invention proposes a distance-aware, dynamically weighted heuristic strategy. First, the weight coefficients of the heuristic terms are dynamically adjusted based on the search progress of the current node. This makes the algorithm more goal-oriented and improves search efficiency in the early stages of the search. In the later stages, the weights of the heuristic terms are reduced to ensure path accuracy, thus balancing search speed and accuracy. Furthermore, a distance estimation method based on diagonal movement is used to construct the heuristic function, making it closer to the actual feasible path cost than Euclidean distance and Manhattan distance, thereby improving the accuracy of path search.

[0062] Based on the hierarchical cost map constructed in step 1, an adaptive multidimensional constraint is adopted. The algorithm generates an initial global path. This experiment uses a 43×75 two-dimensional raster map, where one raster unit represents 1 meter in length. The safe inflation coefficient in the inflation layer is also considered. Set to 2, the row direction obstacle threshold in the orthogonal cumulative occupancy statistics method. Set to 0.1, column direction obstacle threshold Set to 0.6. According to the inspection task requirements, the starting point coordinates are set to (3,35), the passing points are W1(4,7), W2(21,33), W3(34,6), W4(47,33) and W5(59,6) in sequence, and the ending point coordinates are (73,33). The value was set to 2, the robot length was set to 0.6m and the width to 0.53m, and the experimental results are as follows. Figure 3 As shown.

[0063] like Figure 3 As shown, the robot can generate an effective initial global path on the poultry house map environment without colliding with or getting too close to obstacles, verifying the safety and reliability of the global path. Most of the trajectory approaches the center line in the poultry house aisles, which meets the requirements of robot inspection routes in poultry house environment.

[0064] This invention reconstructs the inspection scenario of caged poultry houses by establishing a layered cost map. Global obstacle avoidance is integrated into the expansion layer, and orthogonal cumulative occupancy statistics are used to automatically distinguish the costs of different areas within the risk field layer. Furthermore, the inspection requirement of "driving along the centerline of the passageway" is explicitly encoded into the risk field. The advantage of this approach is that the layered cost map separately models static structural information, real-time obstacle information, safety buffer constraints, and operational behavior constraints in the poultry house environment, and then integrates them uniformly. Simultaneously, after various constraints are pre-encoded into cost layers, they are used... When the global path of the algorithm is regular, the cost can be obtained directly by looking up the table, avoiding repeated complex geometric calculations on the extended nodes, thus balancing security, operational standardization, and computational efficiency.

[0065] Step 3. By using a redundant node removal mechanism, collinear redundant points in the initial global path are eliminated, and only key nodes are retained to generate a globally optimal path node sequence with better geometric features.

[0066] If the global path contains Point, then from the starting point Traverse to the end point Check three consecutive nodes in sequence. , and Are they collinear? If collinear, then the nodes... Delete redundant nodes; keep nodes if the three points are not collinear. .

[0067] Repeat the above checks until all nodes have been traversed, and output the global optimal path node sequence consisting of key nodes.

[0068] Step 4. Select key nodes in the global optimal path node sequence one by one as local navigation targets, and use the improved dynamic window method to select the local optimal trajectory to control the robot's movement and obstacle avoidance until the destination is reached.

[0069] Step 4 primarily transforms the globally optimal path node sequence obtained in Step 3 into a continuous motion control process for the robot during actual inspection. Specifically, it includes the following functions: First, key nodes in the globally optimal path node sequence are identified as local navigation targets. After the robot enters the neighborhood of the current local navigation target point, it switches to the next key node to ensure that the local planning process always progresses segment by segment along the global path direction. Second, considering the robot's current motion state, chassis velocity boundaries, acceleration constraints, obstacle safety distances, and the impact of the telescopic boom's lifting state on overall stability, a feasible speed range using the dynamic window method is constructed. Further, based on the congestion level of the robot's surrounding environment, the sampling resolution of linear and angular velocities is adaptively adjusted, and speed pairs within the feasible speed range are discretely sampled to obtain basic candidate speed pairs. On this basis, using a multi-vertex Jacobian constraint mechanism, the predicted trajectory corresponding to the candidate speed is corrected for dynamic obstacles, generating a variable curvature candidate trajectory. Finally, each candidate trajectory is comprehensively evaluated, the candidate trajectory with the best score is selected, and its corresponding linear and angular velocities are output as control commands for the robot at the next moment, thereby achieving local path tracking and real-time obstacle avoidance.

[0070] During the robot's movement, key nodes from the globally optimal path node sequence output in step 3 are selected one by one as local navigation targets. Let the current local navigation target point be... When the robot enters the neighborhood threshold range of the local navigation target point, it updates the target index to the next key node, performs local path planning and motion control, until it reaches the destination node. The specific process is as follows: Establish the velocity sampling space using the dynamic window method; assuming the robot state is: , ; in This represents the robot's state vector at the current moment. and Here are the robot's position coordinates in the global coordinate system. This indicates the robot's heading angle at the current moment; This is the robot's control vector at the current moment. and Let be the linear velocity and angular velocity at the current moment.

[0071] In multi-layer cage inspection scenarios, the robot is equipped with a telescopic rod carrying multiple cameras. The telescopic rod can adjust the camera height according to the observation needs of cages of different heights to facilitate observation of the situation inside the cage. However, when the telescopic rod is extended, it will cause the robot's center of gravity to rise and the telescopic rod's swing vibration to increase, which will make the acquired images blurry.

[0072] To address this problem, this invention proposes a 2.5D collaborative planning strategy that couples lifting and lowering stability. This strategy dynamically adjusts the upper limit of the linear velocity of the dynamic window based on the extension and retraction of the telescopic rod, defining the steady linear velocity as... As shown below: .

[0073] in This represents the most stable linear velocity measured at the maximum extension height of the telescopic pole. As compensation, This represents the current extension height of the telescopic pole. This represents the maximum extension height of the telescopic pole.

[0074] Based on the boundary constraints of the robot's linear velocity and angular velocity, and the impact of the current lifting height of the lifting mechanism on the chassis stability, the complete velocity set is constructed as follows: .

[0075] .

[0076] in This represents the set of all candidate velocity pairs for the robot under the current operating conditions. Indicates the minimum allowable linear velocity of the robot. This represents the effective maximum linear velocity after considering lifting stability constraints. This indicates the minimum permissible angular velocity of the robot. This indicates the robot's maximum permissible angular velocity; This indicates the robot's maximum linear velocity under normal operating conditions.

[0077] Construct a set of achievable velocities within a control cycle based on the robot's current velocity state and acceleration constraints. : .

[0078] in, This represents the robot's linear velocity at the current moment. This represents the robot's angular velocity at the current moment. Indicates the control cycle; Indicates the maximum linear acceleration; This represents the maximum angular acceleration.

[0079] This invention incorporates the extension height of the telescopic rod into the determination process of the upper limit of the linear velocity in the dynamic window, enabling the robot to reduce its linear velocity when the telescopic rod is in a high position for observation and retain a larger velocity range when the telescopic rod is in a low position. This reduces the blurring of images captured by the camera caused by the increase in the center of gravity and the enhancement of swaying.

[0080] Based on the candidate speed adopted by the robot Distance constraints to the nearest obstacle during movement, constructing a safe speed set. : .

[0081] .

[0082] in Indicates the robot at candidate speeds When performing trajectory prediction, the minimum distance to the nearest obstacle on the predicted trajectory is... Let represent the set of obstacles for the k-th prediction step.

[0083] This represents the predicted position vector of the robot in the k-th prediction step. This represents the position vector of the obstacle.

[0084] By intersecting the above three types of constraints, we obtain the final feasible set of velocities for velocity sampling. : .

[0085] In the feasible velocity set It internally calculates the congestion of the environment and adaptively adjusts the dynamic window sampling resolution.

[0086] Using the forward effective sensing area of ​​the robot's LiDAR as the statistical area for environmental congestion, the total area of ​​this range is denoted as... The area occupied by the obstacle is Then the degree of environmental crowding for: .

[0087] according to Linear velocity sampling number is calculated using linear interpolation. With angular velocity sampling number Target value: ; .

[0088] in and These are the upper and lower limits of the number of linear velocity samples. and These represent the upper and lower limits of the number of angular velocity samples. Increase the sampling density when the environment is more crowded; decrease the sampling density when the environment is more open.

[0089] In the feasible velocity set Inside, according to and Discrete sampling is performed on linear velocity and angular velocity respectively to obtain basic candidate velocity pairs. ; , These are the linear velocity and angular velocity after discrete sampling, respectively. Then, in the basic candidate velocity pair... In addition, a multi-vertex Jacobian constraint mechanism is introduced, enabling the DWA algorithm to perceive dynamic obstacles and gradually correct the angular velocity in the prediction time domain, thereby generating variable curvature candidate trajectories and achieving obstacle avoidance.

[0090] In step 4, the specific process of generating the variable curvature candidate trajectory is as follows: Let the prediction time domain be The control cycle is Then predict the total number of steps. for: .

[0091] In an environment containing dynamic obstacles, assuming the obstacles maintain their current observed velocity and move at a constant speed for a short period, for the s-th dynamic obstacle, let its current observed position vector be denoted as . The current observed velocity vector is Then its position vector at the k-th prediction step Represented as: .

[0092] Define the set of constraint vertex indices for the k-th prediction step. for: .

[0093] in, Indicates the index of the constrained vertex of the main task. This represents the set of indices for the auxiliary constraint vertices.

[0094] For any ,use This represents any constraint vertex that participates in the constraint solution during the current prediction step.

[0095] when hour, Represents the main task constraint vertex This is used to represent the traction constraint of the robot toward a local navigation target point. hour, Indicates the first One activated auxiliary constraint vertex , is used to represent the obstacle avoidance constraints of a robot.

[0096] For any vertex at the k-th prediction step, the position vector of its root in the robot's body coordinate system is denoted as... Then its position vector in the global coordinate system for: ; .

[0097] in Let represent the robot's orientation angle at step k, defined as the angle between the robot's forward axis and the x-axis of the global coordinate system, with counterclockwise being positive. Represents a rotation matrix. and These are the coordinates of the robot's geometric center in the global coordinate system. At least two auxiliary constraint vertices should be set and distributed on both sides of the robot's geometric center.

[0098] Calculate the minimum distance between the root of the auxiliary constraint vertex and the predicted positions of each dynamic obstacle. : .

[0099] in, Set the obstacle avoidance threshold to the number of dynamic obstacles. ,when If the condition is met, the auxiliary constraint vertex is determined to be activated in the current prediction step; otherwise, the auxiliary constraint vertex does not participate in the constraint solution in the current prediction step.

[0100] Establish the Jacobian mapping relationship between the vertex velocity constraint and the chassis angular velocity correction.

[0101] like Figure 4 As shown, for any vertex at the k-th prediction step, its position vector relative to the robot body is described using a two-dimensional equivalent link model. Then, the relative position vector of this vertex in the robot body coordinate system is... for: .

[0102] Among them, the equivalent link lengths are respectively and The angle between the lever and the robot body is The angle between the two rods is Furthermore, the position vector of the vertex in the global coordinate system. for: .

[0103] Main Quest Vertex The desired velocity vector is denoted as As shown below: .

[0104] in, This represents the position vector of the main task vertex in the global coordinate system at step k.

[0105] If an auxiliary constraint vertex is activated, then for any The desired velocity vector of the i-th auxiliary vertex. Set it as the zero vector, and combine the desired velocities of the main task vertex and the auxiliary constraint vertices to form the desired velocity vector. As shown below: .

[0106] For the task velocity vector Mapping to controllable variables of the robot, defining a controllable vector. for: .

[0107] in, This represents the linear velocity of the robot chassis at step k. This represents the angular velocity of the robot chassis at step k; and These represent the angular velocities of the first and second joints of the equivalent two-link linkage corresponding to the vertex of the main task, respectively. and They represent the first The angular velocities of the first and second joints of the equivalent bi-link corresponding to each auxiliary constraint vertex.

[0108] Construct the total Jacobian matrix Calculate the actual velocity vector of each vertex. as follows: . .

[0109] .

[0110] .

[0111] .

[0112] in Let be the block Jacobian matrix of the i-th auxiliary constraint vertex. Let S be the block Jacobian matrix of the main task vertex, and let S denote the fixed antisymmetric matrix. This is the position vector of the main task vertex in the robot's body coordinate system. Let it be represented as the Jacobian matrix of the two links at the i-th endpoint. This represents the bi-link Jacobian matrix corresponding to the vertex of the main task.

[0113] and They are shown below: .

[0114] .

[0115] The controllable vector is obtained by combining pseudo-inverse solving and null space adjustment. , making the actual velocity vector Approximate the desired velocity vector as closely as possible , The solution method is as follows: .

[0116] In the formula Total Jacobian matrix The false rebellion, Represents the identity matrix. This is the spatial adjustment coefficient. This is the state vector of all joint angles of the equivalent two-link linkage in the current step. The reference joint angle vector is constructed for the current step vertex facing the predicted position of the corresponding dangerous obstacle, as follows: In the current number In the prediction step, for any activated auxiliary constraint vertex, select the dynamic obstacle closest to that auxiliary constraint vertex, and place that dynamic obstacle in the prediction step. The predicted position vector for each prediction step is denoted as... ,Will Transform from the global coordinate system to the robot's body coordinate system and calculate its relative to the first... The direction vector at the root of each auxiliary constraint vertex as follows: .

[0117] Then based on the first The two-dimensional equivalent bi-link model corresponding to each auxiliary constraint vertex is used to calculate the second joint reference angle. as follows: ; .

[0118] To avoid numerical errors that would prevent the inverse cosine function from being solved, the above results are saturated: .

[0119] in, This represents a saturation function, used to restrict the input value to a certain range. Within the range.

[0120] First joint reference angle for: .

[0121] in, This represents the arctangent function in the four quadrants.

[0122] Therefore, the first The reference joint angle vectors corresponding to the activated auxiliary constraint vertices are: .

[0123] If the current prediction step has a total of A set of auxiliary constraint vertices is activated, and their index set is: .

[0124] The reference joint angle vector for the current prediction step It can be obtained by stacking the reference joint angle vectors corresponding to each activated auxiliary constraint vertex: .

[0125] From the obtained Extract As the angular velocity correction for the current prediction step, when the obstacle avoidance condition is triggered, the baseline candidate linear velocity is still taken while maintaining the linear velocity. Under the premise of gradually correcting the angular velocity, a variable predicted angular velocity is obtained. for: .

[0126] in, Let be the robot's angular velocity at step k-1. This transforms the candidate trajectory from a constant curvature trajectory to a variable curvature trajectory in the prediction time domain. The formula for generating a candidate trajectory using forward Euler discrete integral is as follows: ; ; .

[0127] All candidate trajectories are scored according to an evaluation function. as follows: .

[0128] in The degree of matching between the trajectory's final heading and the local target's orientation. This represents the minimum distance between the trajectory and the nearest obstacle. For the speed evaluation item, α, β, and γ are the corresponding weighting coefficients. This is the normalization factor.

[0129] , and The expression is as follows: ; ; .

[0130] The candidate predicted trajectory set is formed by all candidate trajectories generated by each basic candidate velocity pair in the prediction time domain.

[0131] The robot selects the candidate trajectory with the highest overall score from the predicted trajectory set and outputs its linear velocity and angular velocity corresponding to the first control cycle as the robot's velocity command for the next moment. After executing the velocity command, the robot updates its pose and environmental perception information in real time. If the destination has not been reached, the iteration continues; if the destination has been reached, the local planning and obstacle avoidance process ends.

[0132] Multi-vertex Jacobian constraint angular velocity correction for dynamic obstacles enables DWA to efficiently avoid dynamic obstacles.

[0133] Step 4 constructs a 2.5D collaborative planning strategy for lifting stability coupling, which reduces the upper limit of linear velocity when the telescopic rod is raised, the robot's center of gravity shifts upward and the oscillation increases, while retaining a large velocity range when the telescopic rod is in a lower position, thereby suppressing attitude disturbances and image blurring caused by high-speed travel.

[0134] Building upon this foundation, environmental congestion is calculated within the feasible velocity set, and the dynamic window sampling resolution is adaptively adjusted. This reduces invalid sampling and computational burden in open environments, while increasing sampling density and improving obstacle avoidance trajectory selection accuracy in congested environments. Furthermore, a multi-vertex Jacobian constraint mechanism is introduced. By constructing a primary task vertex and auxiliary constraint vertices in the prediction time domain, the primary task vertex expresses the traction constraint towards the local navigation target point, while the auxiliary constraint vertices express the avoidance constraint against dynamic obstacles. This transforms the constant curvature prediction trajectory of traditional DWA into a variable curvature candidate trajectory that continuously changes with the prediction time domain. This enables the robot to achieve smoother obstacle avoidance when encountering dynamic obstacles.

[0135] Steps 1 to 4 of this invention constitute a technical solution that connects environmental modeling, global planning, path optimization, and local execution layer by layer. Step 1 constructs a hierarchical cost map to uniformly express the fixed structures, temporary obstacles, safety buffer zones, and operational constraints of different functional areas in the poultry house environment, providing the subsequent planning process with obstacle avoidance criteria and operational behavior constraints. Based on this, Step 2 utilizes the adaptive nature of multidimensional constraints. The algorithm performs a global path search, balancing search speed and accuracy, and improving the accuracy of the path search. Furthermore, step 3 removes collinear redundant nodes from the initial global path, retaining only key nodes, improving the efficiency of subsequent local planning. Based on the above, step 4 uses the obtained key node sequence as local navigation targets sequentially, and combines an improved dynamic window method for real-time trajectory optimization and obstacle avoidance control, ensuring that the global planning results formed in steps 1 to 3 can be continuously and stably executed during actual movement. Since local planning no longer directly targets a single endpoint but progresses segment by segment along key nodes, the robot can still complete local detours and rejoin the global route under global path constraints when encountering temporary obstacles or dynamic disturbances. Simultaneously, step 4 introduces a multi-vertex Jacobian constraint mechanism, generating variable curvature candidate trajectories when encountering dynamic obstacles, enabling the robot to achieve smoother obstacle avoidance. Thus, step 1 provides the basis for constraint expression, step 2 completes global comprehensive optimization, step 3 improves path regularity and traceability, and step 4 realizes dynamic execution and obstacle avoidance for actual working conditions. The four steps work together to improve the reliability of path planning and tracking control of poultry house inspection robots.

[0136] The present invention conducted experimental verification on step 4, and the configuration parameters of the local planning algorithm are shown in Table 1.

[0137] Table 1

[0138] After multiple experiments, the weight configuration of the evaluation function was determined to be as follows: .

[0139] Select the same map environment as the experiment in step 2, process the global path obtained in step 2 through step 3 to obtain the key node sequence, and use it as the local navigation target in step 4.

[0140] The number of linear velocity and angular velocity samples are respectively set as , Maximum extension height of the telescopic pole The most stable linear velocity is 0.5m at the maximum extension height. It is 0.5 m / s. It is 5.

[0141] In this experiment, the origin of the robot's body coordinate system is set at the robot's geometric center, with the forward direction being the positive x-axis and the left side being the positive y-axis. Two auxiliary vertices are used, positioned on the left and right sides of the robot, respectively, and their positions in the robot's body coordinate system are as follows: and Equivalent link length , and , All values ​​are set to 0.25m. The main task vertex is located in front of the robot, and its position in the robot's body coordinate system is... Equivalent link length , Both are 0.2m, obstacle avoidance threshold The initial joint angle of each vertex was set to zero, with a height of 0.5m. Two dynamic obstacles were placed in the cage passage, moving at a constant speed along the horizontal direction. The experimental results are as follows: Figure 5 As shown. By Figure 5 As can be seen, step 4 can use the obtained key node sequence as local navigation targets in sequence, and combine it with the improved dynamic window method to perform real-time trajectory optimization and obstacle avoidance control. The robot can follow the global path well most of the time, and avoid dynamic and static obstacles in real time. After bypassing the obstacles, it returns to the global path in a timely manner, which verifies the effectiveness of the improved dynamic window method.

[0142] Example 2 This embodiment 2 describes a path planning system for an autonomous inspection robot in caged poultry houses, which is based on the same inventive concept as the path planning method for an autonomous inspection robot in caged poultry houses in embodiment 1.

[0143] The path planning system for the autonomous inspection robot in this caged poultry house includes the following modules: The map building module is used to build a two-dimensional raster map, generate a layered cost map, and initialize inspection task information; An initial global path building block for adaptive use with multidimensional constraints. The algorithm plans the initial global path on a hierarchical cost map; The global optimal path generation module is used to remove collinear redundant points in the initial global path through a redundant node removal mechanism, retaining only key nodes, in order to generate a global optimal path node sequence with better geometric features. The system also includes a navigation control module, which selects key nodes from the global optimal path node sequence one by one as local navigation targets, uses an improved dynamic window method to optimize the local optimal trajectory, and controls the robot's movement and obstacle avoidance until it reaches the destination.

[0144] It should be noted that any content not mentioned in the above-described functional modules of the system described in Embodiment 2 can be referred to the step description of the corresponding method in Embodiment 1 above, and will not be repeated in detail here.

[0145] Example 3 This embodiment 3 describes a computer device, which includes a memory and one or more processors. Executable code is stored in the memory. When the processor executes the executable code, it implements the steps of the path planning method for the autonomous inspection robot in caged poultry houses described in embodiment 1 above.

[0146] This invention uses human-computer interaction to issue operation instructions to computer devices in either full-coverage inspection mode or point-to-point fixed-point inspection mode, and displays the constructed grid map, planned path, and the robot's current operating status in real time.

[0147] Computer equipment performs adaptive multidimensional constraint based on received sensor data. Algorithm and improved dynamic window method: Generate a global optimal path node sequence according to the inspection task requirements, and calculate the optimal linear velocity and angular velocity control command for the next moment by combining real-time environmental data; The optimal linear velocity and angular velocity control command drives the robot to travel along the planned path by adjusting the speed difference between the left and right wheels, and performs straight-line, turning and obstacle avoidance actions.

[0148] In this embodiment, the computer device can be any device or apparatus with data processing capabilities, and will not be described in detail here.

[0149] Example 4 This embodiment 4 describes a computer-readable storage medium storing a program that, when executed by a processor, is used to implement the steps of the path planning method for the autonomous inspection robot of caged poultry houses in embodiment 1 above.

[0150] The computer-readable storage medium can be an internal storage unit of any device or apparatus with data processing capabilities, such as a hard disk or memory, or an external storage device of any device with data processing capabilities, such as a plug-in hard disk, smart media card (SMC), SD card, flash card, etc.

[0151] Of course, the above description is only a preferred embodiment of the present invention. The present invention is not limited to the above-described embodiments. It should be noted that any equivalent substitutions or obvious modifications made by those skilled in the art under the guidance of this specification fall within the scope of this specification and should be protected by the present invention.

Claims

1. A path planning method for an autonomous inspection robot in a caged poultry house, characterized in that, Includes the following steps: Step 1. Construct a two-dimensional grid map, generate a layered cost map, and initialize the inspection task information; Step 2. Adaptive use of multidimensional constraints The algorithm plans the initial global path on a hierarchical cost map; Step 3. By using a redundant node removal mechanism, collinear redundant points in the initial global path are eliminated, and only key nodes are retained to generate a globally optimal path node sequence with better geometric features; Step 4. Select key nodes in the global optimal path node sequence one by one as local navigation targets, and use the improved dynamic window method to select the local optimal trajectory to control the robot's movement and obstacle avoidance until the destination is reached; In step 1, the layered cost map comprises four independent data layers: a static map layer, a dynamic obstacle layer, an inflation layer, and a risk field layer; each data layer is formed as follows: The static map layer is a two-dimensional occupancy grid reference map pre-generated based on the known structural environment of the caged poultry house and the prior environmental information of the fixed cage frame layout, through scale calibration, coordinate mapping and raster modeling; The dynamic obstacle layer detects temporary obstacles in the environment based on the environmental information perceived in real time by the robot's sensors, and marks the grid corresponding to the detected temporary obstacles as occupied, so as to realize the real-time update of the cost map. The expansion layer is based on the obstacle boundaries in the static map layer and the dynamic obstacle layer, and a safe width is established outward along the obstacle boundaries. A buffer zone is provided to prevent the robot from running close to the edge; among which ; The safety expansion coefficient is used to characterize the proportional relationship of the outward expansion of the obstacle boundary, and ; The equivalent circle radius of the robot satisfy ; For vehicle body length, This refers to the width of the vehicle body; Let the occupancy status of each grid cell in the layered cost map be... , ,when When, it indicates that the grid cell is an obstacle grid. When the time is right, it indicates that the grid cell is an empty grid cell; For each obstacle grid, the distance from its boundary shall not exceed The grid markings indicate impassable areas, forming an expansion layer; In addition, a risk field layer is established in the cost map, and different constraints are assigned to different regions; In step 1, the process of establishing a risk field layer in the cost map and assigning differentiated constraints to different regions is as follows: Based on the inspection task requirements, the robot's operating area is divided into cage passage areas. With the transfer area ; Automatically distinguish between cage passage areas using orthogonal cumulative occupancy statistics With the transfer area The process is as follows: Let the occupancy status of each raster cell in the static map layer be denoted as . ; in And set the map width of the static map layer to be , length is ; Calculating horizontal edge functions based on static map layers : ； in Indicates the first static map layer The number of grid cells occupied by obstacles in a row; Will satisfy The row is used as a candidate job row. This represents the row-direction obstacle threshold, used to indicate the ratio between the number of grid cells occupied by obstacles in the y-th row and the total number of horizontal grid cells. ; Sort all candidate job rows in ascending order of row index. If the difference between the row indexes of two adjacent candidate job rows is 1, they are grouped into the same continuous row interval; otherwise, a new continuous row interval is opened, and finally, the candidate continuous row interval is obtained. Let the starting row index of the candidate continuous row interval be denoted as The terminating row index is denoted as ; interval express The upper and lower boundaries of the region are determined; subsequently, the statistical function of the vertical edge is calculated. as follows: ； in Indicates the x-th column in the static map layer The number of grid cells occupied by internal obstacles; The vertical span of the region is ; Based on this, construct a candidate column set. as follows: ;in The threshold for obstacle in the column direction, and , set of candidate columns After sorting by x in ascending order, interval clustering is performed based on the continuity of the column index; If the row index difference between two adjacent candidate columns is 1, then they are classified into the same continuous column range; Otherwise, initialize a new column range to obtain the inter-cage aisle column range. ; in , They represent the first The start and end positions of the aisle section between bar cages; Based on the transverse interval of the cage passageway and work area The upper and lower boundaries automatically divide the map grid into cage passage areas. With the transfer area ,in: ； ； in This represents the number of cage passageways identified. Grids that meet the above conditions are respectively assigned to the cage passageway area. and transfer area The risk level is in the cage passageway area. Using the centerline of the cage passage as a reference, a risk potential field is constructed that increases non-linearly from the inside out according to the lateral distance from the grid nodes to the centerline; in the turning area Within this context, the risk-value is set to zero. For any grid node Its risk potential energy is defined as : ； in Risk amplitude coefficient and , , , ; Pre-calculation of the inter-cage passage area The risk value of each grid cell within the region is calculated, and a risk field layer matrix with the same resolution and size as the global grid map is formed. , This is the lower limit of the channel half-width; The risk cost of grid nodes in node expansion is borne by Lookup table; Step 2 specifically involves: Adaptive The algorithm uses nodes in the raster map as path search objects, establishes Open and Closed tables, and traverses and filters the neighboring nodes of the current node; for neighboring nodes that meet the passability conditions, its path cost is calculated. During the path search process, for any candidate node Construct a comprehensive evaluation function : ； in Indicates the distance from the starting point to the node. The cumulative actual cost, Indicates from node Heuristic cost estimation to the target node For the dynamic weighting coefficients of the heuristic term; Represents a node The risk value in the risk field layer; According to the node The corresponding raster coordinates are derived from the pre-constructed risk field layer matrix. Obtained from the middle; Heuristic function Distance estimation based on diagonal movement is used, and its calculation formula is as follows: ； ； in The cost of a robot moving one unit distance. The coordinates of the target node. The coordinates of the current node; dynamic weight coefficients. The calculation method is as follows: The system adaptively adjusts based on the search progress. ； Adaptive The algorithm selects from the Open table during the iteration process. The smallest node is selected as the current expansion node, and its neighboring nodes are calculated and updated accordingly. , and And maintain the Open and Closed tables; When the target node is selected or the preset termination condition is met, the initial global path is obtained by backtracking based on the parent node pointer.

2. The path planning method for the autonomous inspection robot of caged poultry houses according to claim 1, characterized in that, Step 3 specifically involves: If the global path contains Point, then from the starting point Traverse to the end point Check three consecutive nodes in sequence. , and Are they collinear? If collinear, then the nodes... Delete redundant nodes; If the three points are not collinear, then the node is retained. ; Repeat the above checks until all nodes have been traversed, and output the global optimal path node sequence consisting of key nodes.

3. The path planning method for the autonomous inspection robot of caged poultry houses according to claim 1, characterized in that, Step 4 specifically involves: Let the position vector of the current local navigation target point be... When the robot enters the neighborhood threshold range of the target point, it updates the target index to the next key node, performs local path planning and motion control, and continues until the destination node is reached. The specific process is as follows: Establish the velocity sampling space using the dynamic window method; assuming the robot state is: ， ； in This represents the robot's state vector at the current moment. and Let be the robot's position coordinates in the global coordinate system at time t. This represents the robot's heading angle at time t; This is the robot's control vector at the current moment. and Let be the linear velocity and angular velocity at the current moment; In multi-layer cage inspection scenarios, the robot is equipped with a telescopic rod carrying multiple cameras. The upper limit of the linear velocity of the dynamic window is dynamically adjusted based on the extension and retraction of the telescopic rod. A steady linear velocity is defined as... As shown below: ； in This represents the most stable linear velocity measured at the maximum extension height of the telescopic pole. As compensation, This represents the current extension height of the telescopic pole. The maximum extension height of the telescopic rod; based on the boundary constraints of the robot's linear velocity and angular velocity, and the impact of the current lifting height of the lifting mechanism on the chassis stability, the full velocity set is constructed as follows: ； ； in This represents the set of all candidate velocity pairs for the robot under the current operating conditions. This indicates the minimum allowable linear velocity for the robot. This represents the effective maximum linear velocity after considering lifting stability constraints. This indicates the minimum permissible angular velocity for the robot. This indicates the robot's maximum permissible angular velocity; This indicates the robot's maximum linear velocity under normal operating conditions; Construct a set of achievable velocities within a control cycle based on the robot's current velocity state and acceleration constraints. : ； in, This represents the robot's linear velocity at the current moment. This represents the robot's angular velocity at the current moment. Indicates the control cycle; Indicates the maximum linear acceleration; Indicates the maximum angular acceleration; Based on the candidate speed adopted by the robot Distance constraints to the nearest obstacle during movement, constructing a safe speed set. : ； ； in Indicates the robot at candidate speeds The minimum distance to the nearest obstacle on the predicted trajectory when performing trajectory prediction. Let represent the set of obstacle position vectors for the k-th prediction step; This represents the predicted position vector of the robot in the k-th prediction step. Represents the position vector of the obstacle; By intersecting the above three types of constraints, we obtain the final feasible set of velocities for velocity sampling. : ； In the feasible velocity set The system calculates the congestion level of the internal computing environment and adaptively adjusts the dynamic window sampling resolution. Using the forward effective sensing area of ​​the robot's LiDAR as the statistical area for environmental congestion, the total area of ​​this range is denoted as... The area occupied by the obstacle is Then the degree of environmental crowding for: ； according to Linear velocity sampling number is calculated using linear interpolation. With angular velocity sampling number Target value: ； ； in and These are the upper and lower limits of the number of linear velocity samples. and These are the upper and lower limits of the number of angular velocity samples. In the feasible velocity set Inside, according to and Discrete sampling is performed on linear velocity and angular velocity respectively to obtain basic candidate velocity pairs. ; , These are the linear velocity and angular velocity after discrete sampling, respectively; Subsequently, in the basic candidate velocity pair In addition, a multi-vertex Jacobian constraint mechanism is introduced, enabling the DWA algorithm to perceive dynamic obstacles, gradually correct the angular velocity in the prediction time domain, and generate variable curvature candidate trajectories.

4. The path planning method for the autonomous inspection robot of caged poultry houses according to claim 3, characterized in that, In step 4, the specific process of generating the variable curvature candidate trajectory is as follows: Let the prediction time domain be The control cycle is Then predict the total number of steps. for: ; In an environment containing dynamic obstacles, assuming the obstacles maintain their current observed velocity and move at a constant speed for a short period, for the s-th dynamic obstacle, let its current observed position vector be denoted as . The current observed velocity vector is ; Then its position vector at the k-th prediction step Represented as: ； Define the set of constraint vertex indices for the k-th prediction step. for: ;in, Indicates the index of the constrained vertex of the main task. The set of indices representing the vertices of the auxiliary constraint; For any ,use This represents any constraint vertex that participates in the constraint solution during the current prediction step; when hour, Represents the main task constraint vertex , used to represent the traction constraint of the robot toward the local navigation target point; when hour, Indicates the first An active auxiliary constraint vertex is used to represent the robot's obstacle avoidance constraints; For any vertex at the k-th prediction step, the position vector of its root in the robot's body coordinate system is denoted as... Then its position vector in the global coordinate system for: ， ； in Let represent the robot's orientation angle at step k, defined as the angle between the robot's forward axis and the x-axis of the global coordinate system, with counterclockwise being positive. Represents a rotation matrix; and These are the coordinates of the robot's geometric center in the global coordinate system; at least two auxiliary constraint vertices should be set and distributed on both sides of the robot's geometric center; Calculate the minimum distance between the root of the auxiliary constraint vertex and the predicted position of each dynamic obstacle. : ； in, Set the obstacle avoidance threshold to the number of dynamic obstacles. ,when If the auxiliary constraint vertex is activated in the current prediction step, then the auxiliary constraint vertex is determined to be activated in the current prediction step; otherwise, the auxiliary constraint vertex does not participate in the constraint solution in the current prediction step. Establish the Jacobian mapping relationship between the vertex velocity constraint and the chassis angular velocity correction; For any vertex at the k-th prediction step, if its position vector relative to the robot body is described using a two-dimensional equivalent link model, then the relative position vector of that vertex in the robot body coordinate system is... for: ； in, and These represent the lengths of the first link and the second link, respectively. The angle between the first link and the positive x-axis of the robot's body coordinate system is... The angle between the second link and the first link is... ; Furthermore, the position vector of the vertex in the global coordinate system for: ； Main Quest Vertex The desired velocity vector is denoted as As shown below: ； in This represents the position vector of the main task vertex in the global coordinate system at step k; If an auxiliary constraint vertex is activated, then for any The desired velocity vector of the i-th auxiliary vertex. Set it as the zero vector, and combine the desired velocities of the main task vertex and the auxiliary constraint vertices to form the desired velocity vector. as follows: ； To the desired velocity vector Mapping to controllable variables of the robot, defining a controllable vector. for: ； in This represents the linear velocity of the robot chassis at step k. This represents the angular velocity of the robot chassis at step k; and These represent the angular velocities of the first and second joints of the equivalent two-link linkage corresponding to the vertex of the main task, respectively. and They represent the first The angular velocities of the first and second joints of the equivalent two-link rod corresponding to each auxiliary constraint vertex; Construct the total Jacobian matrix Calculate the actual velocity vector of each vertex. as follows: ； ； ； ； ； in Let be the block Jacobian matrix of the i-th auxiliary constraint vertex. Let S be the block Jacobian matrix of the main task vertex, and let S denote the fixed antisymmetric matrix. This is the position vector of the main task vertex in the robot's body coordinate system. Let it be represented as the Jacobian matrix of the two links at the i-th endpoint. This represents the bi-link Jacobian matrix corresponding to the vertex of the main task; and They are shown below: ； ； The controllable vector is obtained by combining pseudo-inverse solving and null space adjustment. , making the actual velocity vector Approximating the desired velocity vector , The solution method is as follows: ; In the formula Total Jacobian matrix The false rebellion, Represents the identity matrix. This is the spatial adjustment coefficient. This is the state vector of all joint angles of the equivalent two-link linkage in the current step. A reference joint angle vector is constructed for the current step vertex to face the predicted position of the corresponding dangerous obstacle; From the obtained Extract As the angular velocity correction for the current prediction step, when the obstacle avoidance condition is triggered, the baseline candidate linear velocity is still taken while maintaining the linear velocity. Under the premise of gradually correcting the angular velocity, a variable predicted angular velocity is obtained. for: ； in, Let be the robot's angular velocity at step k-1. This transforms the candidate trajectory from a constant curvature trajectory to a variable curvature trajectory in the prediction time domain. The formula for generating a candidate trajectory using forward Euler discrete integral is as follows: ； ； ； All candidate trajectories are scored according to an evaluation function. as follows: ； in This refers to the degree of matching between the trajectory's final heading and the local target's orientation. This represents the minimum distance between the trajectory and the nearest obstacle. For the speed evaluation item, α, β, and γ are the corresponding weighting coefficients. Normalization factor; , and The expression is as follows: ; ; ; The position of the end of the trajectory is denoted as The local navigation target point is denoted as ; Indicates the heading angle at the end of the candidate trajectory. This represents the target direction angle from the end position of the candidate trajectory to the current local navigation target point. This means normalizing the angle difference to... Within the range; The candidate trajectories generated by each basic candidate velocity pair within the prediction time domain constitute the candidate prediction trajectory set. Select the candidate trajectory with the highest comprehensive score from the predicted trajectory set, and output the linear velocity and angular velocity corresponding to its first control cycle as the velocity command for the robot at the next moment; After the robot executes the speed command, it updates its pose and environmental perception information in real time. If it has not reached the destination, it continues to iterate. If it has reached the destination, it ends the local planning and obstacle avoidance process.

5. A path planning system for an autonomous inspection robot in a caged poultry house, used to implement the path planning method for an autonomous inspection robot in caged poultry houses as described in any one of claims 1 to 4, characterized in that, The path planning system for the autonomous inspection robot in caged poultry houses includes the following modules: The map building module is used to build a two-dimensional raster map, generate a layered cost map, and initialize inspection task information; An initial global path building block for adaptive use with multidimensional constraints. The algorithm plans the initial global path on a hierarchical cost map; The global optimal path generation module is used to remove collinear redundant points in the initial global path through a redundant node removal mechanism, retaining only key nodes, in order to generate a global optimal path node sequence with better geometric features. The system also includes a navigation control module, which selects key nodes from the global optimal path node sequence one by one as local navigation targets, uses an improved dynamic window method to select the local optimal trajectory, controls the robot's movement and obstacle avoidance, until it reaches the destination.

6. A computer device, comprising a memory and one or more processors; characterized in that, The memory stores executable code, which, when executed by the processor, is used to implement the path planning method for the autonomous inspection robot of caged poultry houses as described in any one of claims 1 to 4.

7. A computer-readable storage medium having a program stored thereon; characterized in that, When executed by the processor, the program is used to implement the path planning method for the autonomous inspection robot of caged poultry houses as described in any one of claims 1 to 4.

Images