Multi-agent self-organizing path planning and positioning technology based on unknown area

Abstract

The invention relates to the field of indoor path planning and positioning, in particular to a multi-agent self-organizing path planning and positioning technology based on an unknown region. Comprising the following steps: enabling multiple robots to enter an unknown space in a formation form by using a navigation-following method; continuously acquiring obstacle information and target position information in a space by using a laser radar carried by the intelligent agent; based on continuously updated known information, an exploration type A * algorithm and an improved DWA algorithm are fused to jointly guide obstacle avoidance and path planning; in the exploration process, multiple agents are distributed through visual chain connection, and the whole exploration area is covered to achieve area monitoring; the laser radar coordinate systems of all the intelligent agents are converted to the same world coordinate system through coordinate conversion, and multi-laser radar view angle fusion is achieved; in a monitoring environment constructed by an intelligent agent cluster, an indoor moving target is tracked and monitored, the target is positioned after being stopped, and finally tracking, monitoring and positioning of the moving target in an unknown area are achieved.

Description

Technical Field

[0001] This application relates to the field of indoor positioning, and particularly to a self-organizing path planning and positioning technology for multiple agents in an unknown area. Background Art

[0002] In recent years, with the rise of information perception technology, positioning and path planning have received extensive attention. Traditional path planning requires a large amount of known information, which is not available in a limited environment. This paper designs a self-organizing motion scheme for multi-agent vehicles with a complex indoor space where GPS cannot be used, including exploration and path planning. By improving the DWA algorithm and the A* algorithm, multi-robot self-organization realizes reasonable path planning, and the fusion of the two algorithms solves the contradiction problem that global planning cannot avoid dynamic obstacles and local planning may fall into local optimality. Then, the pilot-follow algorithm is added to guide multi-agent formation operation. By studying the constraint conditions of hardware such as lidar and robotic vehicles, a chain distribution of multi-intelligences is proposed to solve the problem of information loss caused by discontinuous monitoring fields of view. Finally, when all vehicles are in place, the entire area will be covered, and multi-viewpoint sensor fusion is used for monitoring. Simulation experiments verify the feasibility of the explored scheme, and specific hardware verifies the feasibility and robustness of multi-sensor fusion. Summary of the Invention

[0003] This application provides a self-organizing path planning and positioning technology for multiple agents in an unknown area, which can solve the path planning and positioning problems such as blind spots, signal interruption, information loss, and poor mobility in indoor positioning.

[0004] The technical solution of this application is an indoor positioning method for an unknown area based on multiple self-organizing lidars, including:

[0005] S1: Using the pilot-follow algorithm, enabling multiple robots to start from the same point and enter the unknown space in formation;

[0006] Including the leader using known information to lead the following vehicles for exploration, obstacle avoidance, and selecting a suitable path through decision-making;

[0007] S2: Improving the A* algorithm, improving the DWA algorithm and fusing them to jointly guide exploration and obstacle avoidance;

[0008] Including improving the traditional A* algorithm so that it no longer requires global map information, but uses known information for exploratory path planning, and realizing the avoidance of dynamic and static obstacles by establishing a new evaluation model of DWA, and providing a suitable path and distance for the subsequent agent distribution;

[0009] S3: Use the visual chain distribution strategy to guide multiple agents to self-organize and distribute according to the exploration scenario during exploration;

[0010] This includes enabling multiple agents to achieve distribution in a chain structure through a large number of constraints and judgment conditions, and solving problems such as the inability to use wireless signals in enclosed spaces, resulting in unknowns and information loss, through the distribution form.

[0011] S4: Take the laser coordinate system of the rear vehicle lidar itself as the world coordinate system, with the lidar itself as the origin, and convert the laser coordinate systems of the other agents through the rotation matrix and merge them into the world coordinate system to achieve the perspective fusion function of multiple lidars;

[0012] S5: Use perspective fusion to comprehensively observe the movement route and hiding position of the moving target, calculate and confirm the position after the target stops to facilitate subsequent operations.

[0013] The beneficial effects obtained by the present invention using the above structure are as follows: A self-organizing path planning and positioning technology for multiple agents based on unknown regions proposed by this solution has the following beneficial effects:

[0014] Traditional methods usually use a single lidar device to achieve indoor positioning, which is greatly affected by obstacles and lighting conditions, and cannot perform positioning and monitoring work in unknown regions and enclosed environments. Different from traditional methods, the present invention uses a multi-agent lidar system, where multiple devices work together and complement each other to provide multi-source data, thus significantly improving the accuracy and robustness of positioning. Moreover, it uses the improved A* and improved DWA fusion to guide the agent formation to cooperate in obstacle avoidance and exploration, achieving global path planning and dynamic path planning, and getting rid of the problems in traditional methods that require a large amount of information or are prone to falling into local optima. BRIEF DESCRIPTION OF THE DRAWINGS

[0015] In order to more clearly illustrate the technical solutions of the present application, the drawings required for use in the embodiments will be briefly introduced below. Obviously, for those of ordinary skill in the art, other drawings can also be obtained based on these drawings without creative efforts.

[0016] Figure 1 It is a flow chart of the exploration and positioning principle of a self-organizing path planning and positioning technology for multiple agents based on unknown regions of the present invention;

[0017] Figure 2 It is a graph of the motion parameters of agents of a self-organizing path planning and positioning technology for multiple agents based on unknown regions of the present invention;

[0018] Figure 3It is a leader-follower motion parameter diagram of a self-organizing path planning and positioning technology for multiple agents in an unknown area according to the present invention;

[0019] Figure 4 It is an exploratory A* algorithm diagram of a self-organizing path planning and positioning technology for multiple agents in an unknown area according to the present invention;

[0020] Figure 5 It is an avoiding dynamic obstacle diagram of a self-organizing path planning and positioning technology for multiple agents in an unknown area according to the present invention;

[0021] Figure 6 It is a distribution strategy diagram of a self-organizing path planning and positioning technology for multiple agents in an unknown area according to the present invention;

[0022] Figure 7 It is a multi-agent perspective stitching diagram of a self-organizing path planning and positioning technology for multiple agents in an unknown area according to the present invention. Detailed implementation manners

[0023] The embodiments will be described in detail below, and the examples are shown in the drawings. When the following description refers to the drawings, unless otherwise indicated, the same numbers in different drawings represent the same or similar elements. The implementation manners described in the following embodiments do not represent all the implementation manners consistent with the present application. They are only examples of the systems and methods consistent with some aspects of the present application detailed in the claims.

[0024] The steps of the specific target positioning function are as follows:

[0025] (1) After all the above preparations are completed, when a moving target enters the monitored area, all the lidars that can scan the target will simultaneously update the information on the computer, and the contour of the moving target in the indoor space will be displayed on the Rviz of the computer and its position will change continuously.

[0026] (2) After the moving target stops moving, it can be accurately monitored regardless of its position relative to the obstacles. At this time, different lidars will give different angular distance information for calculating the specific position of the target to be measured. Through the measurement of multiple lidars, a more reasonable and accurate position is finally selected.

[0027] The above has described the embodiments of the present application in detail, but the content is only the preferred embodiments of the present application, and cannot be considered as limiting the scope of implementation of the present application. Any equal changes and improvements made within the scope of the embodiments of the present application should still fall within the scope covered by the patent of the embodiments of the present application.

Claims

1. A self-organizing path planning and positioning technology for multiple intelligent agents in an unknown area, characterized in that: include: S1: Using the leader-follower algorithm, multiple robots start from the same point and enter the unknown space in formation; This includes the leader using known information to lead followers to explore, avoid obstacles, and choose the appropriate path through decision-making; S2: Improve the A* algorithm and the DWA algorithm and integrate them to jointly guide exploration and obstacle avoidance; This includes improving the traditional A* algorithm so that it no longer needs global map information for path planning, but uses known information for exploratory path planning, and avoids dynamic obstacles by establishing a new DWA evaluation model, and provides appropriate paths and distances for subsequent intelligent agent distribution; integrating the exploratory A* algorithm with the improved DWA algorithm solves the problem that a single A* algorithm is difficult to handle dynamic information, and at the same time uses the global planning capability of the A* algorithm to solve the problem that DWA is prone to falling into local optimality when calculating paths; S3: Use visual chain distribution strategy to guide multi-agents to self-organize distribution according to exploration scenarios during exploration; This includes using a large number of constraints and judgment conditions to enable multiple agents to be distributed in a chain structure. When wireless communication between agents or with the outside world fails due to various problems in the space, information is transmitted step by step through the chain structure, solving the problem of position and information loss caused by the inability to use wireless signals in a confined space through a distributed form. S4: Use the rear vehicle laser radar itself as the origin to establish a world coordinate system. Use the rotation matrix to transform the laser coordinate systems of the other agents and merge them into the world coordinate system to achieve the multi-laser radar perspective fusion function. Through perspective fusion, we can solve the problems of blind spots of obstacles, reflections, and inaccurate positioning of a single laser radar. S5: Use perspective fusion to comprehensively observe the moving target's moving route and hiding position, and use the information collected by the radar to calculate and confirm the position of the target after it stops, to facilitate subsequent operations.

2. The self-organizing path planning and positioning technology of multiple intelligent agents based on unknown areas according to claim 1 is characterized in that: The step S1 comprises: S11: Lead-Follow Through the leader-follower strategy, multi-agent formations are able to enter and explore unknown spaces; the kinematic equations of multi-agents are first considered, and the Mecanum wheel is used as the motion device of the agent; For the leader-follower method, the leading vehicle leads and the following vehicles follow. During the formation exploration process, the following vehicle does not simply copy the driving trajectory of the leading vehicle, that is, following the track, but calculates a path that can both follow and ensure safety based on the moving direction, relative distance, and various speeds of the leading vehicle. This strategy not only enables the following vehicle to effectively follow the leading vehicle, but also has a built-in anti-collision mechanism, thereby ensuring the safety and efficiency of the entire formation process; Follow Robot R F The final destination is the virtual robot R V The location is not the location of the pilot robot R L The position of R L Indicates the pilot robot, R V represents a virtual robot; Since the virtual robot R V The position of the pilot robot R L The position of the navigator changes, so it cannot be determined at the beginning. It needs to be calculated based on the position of the navigator and the corresponding relationship between the navigator robot and the virtual robot. The specific steps are as follows: x V ,y V is the horizontal and vertical coordinates of the virtual robot; x L ,y L is the horizontal and vertical coordinates of the virtual robot, d LV is the length of the line between the pilot robot and the virtual robot center; d LF is the length of the line between the center of the leader robot and the follower robot; is the x direction of the pilot robot and d LV The angle between L is the angle between the x direction and the positive X direction of the pilot robot, is the x direction of the pilot robot and d LF The angle between F Yes LF The angle between the positive X direction and the V is the angle between the virtual robot's x direction and the positive X direction; Using the above results, the virtual robot R V and follow robot R F However, the movement of multiple agents involves multiple issues such as perception, control, movement, and environment, so the virtual robot R V and follow robot R F There must be a slight difference between them, that is, the state error: x eVF ,y eVF ,θ eVF is the state error between the virtual robot and the following robot in horizontal and vertical coordinates and angles; V 、x F ,y V ,y F are the horizontal and vertical coordinates of the virtual robot and the follower robot respectively; The state errors of the virtual robot and the follower robot in the global coordinate system are converted into the reference coordinate system through the transfer matrix, and the state error expression is obtained: Among them, e x 、e y 、e θ They are the errors in the x and y directions and the angle errors respectively; Substituting (1) and (2) into (4), we obtain: The state error should be minimized or eliminated through the control system, so the relationship between the state error and the linear velocity and angular velocity of the follower is obtained by taking the derivative of equation (5): ω L ,ω F are the angular velocities of the leading robot and the following robot respectively; by substituting into the motion equation (1), the controller is used to make Closest to zero.

3. The self-organizing path planning and positioning technology of multiple intelligent agents based on unknown areas according to claim 1 is characterized in that: The step S2 comprises: (1) Exploratory A* algorithm: In view of the fact that the traditional A* algorithm relies on complete environmental map information, the exploratory A* algorithm is introduced; the algorithm works in an unknown environment, obtains local information of the surrounding environment in real time through sensors such as lidar, and dynamically sets temporary target points within its scanning range; as the mobile robot continues to explore and move forward, these temporary target points are constantly updated, guiding the robot to gradually approach the global goal, and while exploring, the route passed is recorded to avoid secondary exploration as much as possible, while achieving efficient exploration and monitoring of unknown space; This strategy not only overcomes the limitation of the traditional A* algorithm that cannot be directly applied in unknown environments, but also improves the robot's autonomous exploration ability in unknown environments; 1. Select a local target position P within the scanning range local , using the A* algorithm from the current position P current To the target position P local Perform path planning. f(n)=g(n)+h(n) (7) f(n) is the total cost of node n; g(n) is the sum of the values ​​from the starting point P current The actual cost to reach node n; h(n) is the distance from node n to the local target position P local Heuristic estimation cost of ; When the car reaches the local target position or finds that the target position cannot be reached during the process, the target position is updated, using the Euclidean distance as the heuristic function: scan(P current ,R) indicates that P current The set of points within the scanning range with a radius of R as the center; use the above formula to update a new target position, and repeat this process to achieve the purpose of exploring the unknown space; 2. Local target point selection: p*=select_local_goal(P local ,current_position) (9) P local is the local target position found in the A* algorithm, and p* is the local target position found in the path of the A* algorithm in the DWA algorithm. Both are in a state of being dynamically updated; (2) DWA dynamic window method: The core idea of ​​the DWA algorithm is to determine a sampling speed space that satisfies the hardware constraints of the mobile robot in the speed space (v, ω) according to the current position state and speed state of the mobile robot, and then calculate the trajectory of the mobile robot moving within a certain period of time under these speed conditions, and calculate the score of the trajectory through the evaluation function, and finally select the speed corresponding to the best evaluated trajectory as the movement speed of the mobile robot, and repeat this cycle until the mobile robot reaches the target point; Trajectory scoring function: The trajectory scoring function is mainly used to calculate the scores of multiple paths, and use the cost scoring to select the best and most reasonable path. The ultimate goal of this application is to achieve path planning of multiple agents in unknown space and the reasonable distribution of agents, with higher requirements for obstacle avoidance, anti-collision effect and smoothness of obstacle avoidance; Improved evaluation function of DWA algorithm: α, β, γ, δ are weight coefficients, g goal , g velocity , g obstacle , g direction They are target score, smoothness score, obstacle score, and direction score; 1. The target score in formula (9) represents the distance between the current state position of the robot and the target position. The goal of the system is to minimize this distance. The smaller the distance, the higher the score, which is used to guide the robot to move to the target position. g goal (v,ω)=α·dist(p(v,ω),goal) (11) dist(p(v, ω), goal) is the distance between the robot's predicted position and the target position at velocity v and angular velocity ω. The specific calculation method is: Using Euclidean distance calculation, we can get the distance between the current position of the car and the target position, x goal ,y goal is the horizontal and vertical coordinates of the target position; 2. The smoothness score indicates the smoothness of the speed and angle changes of the agent during movement. The goal of the system is to minimize the rate of change of speed and angle while achieving the function. The smoother the change, the better. is the rate of change of velocity; is the rate of change of angular velocity; 3. The obstacle score represents the distance score between the agent and the obstacle. Its goal is to keep the agent within a certain distance from the obstacle as much as possible under the minimum obstacle avoidance requirement. It should not be too far away from the obstacle, resulting in reduced obstacle avoidance efficiency, nor too close to the obstacle, resulting in limited LiDAR detection field of view. min_dist(p(v,ω),obstacles) is the minimum distance between the robot’s predicted position and the nearest obstacle at velocity v and angular velocity ω; ε is a small value used to avoid the denominator being zero; In the above formula, a simple Euclidean distance calculation is used to make the car as far away from the obstacle as possible to achieve the obstacle avoidance function. However, the function to be achieved in this application is not simply the farther away from the obstacle, the better. The function achieved in this application also involves multiple intelligent distribution and spatial monitoring. In order to achieve the best monitoring effect, it is necessary not only to consider avoiding obstacles, but also to consider that the monitoring angle will become narrower when the car is close to the obstacle, and the problem of insufficient coverage of the obstacle will occur if the car is too far away from the obstacle. Therefore, a new obstacle score formula is adopted, which calculates the score by the distance to the obstacle in different distance levels, and the score changes at different levels at different speeds; The above formula is used to calculate the minimum distance min_dist from each point on the trajectory to the nearest obstacle, and then use the exponential decay function to calculate the score. When min_dist is very small, the score decreases very quickly; when min_dist increases, the score decreases more slowly; μ is the appropriate distance that is expected to be maintained, which is related to the number of robots deployed and the complexity of the space; λ controls the decay speed; a larger λ value will keep the score higher within a larger distance range. It is adjusted according to the requirements of distance sensitivity. The larger the λ is, the slower the score decreases and the wider the applicable range; the smaller the λ is, the faster the score decreases and the narrower the applicable range. Using this formula, the robot car can achieve obstacle avoidance within a certain distance range.

4. The direction score indicates the angle between the direction after obstacle avoidance and the target direction. The goal is to minimize this angle. The smaller the angle, the higher the score, indicating that the vehicle can continue to move in the original direction after avoiding the obstacle. H(v,ω)=δ·(-|angle_diff(θ goal ,the new )|) (17) θ goal =arctan(and goal -y,x goal -x) i new =θ+ω·Δt θ goal is the angle between the robot’s current direction and the direction toward the target position; θ new is the angle between the new direction of the robot at velocity v and angular velocity ω and the direction toward the target position; angle_diff(θ goal ,θ new ) is θ goal With θ new The angle difference between Dynamic Window Constraints: In DWA, a dynamic window defines a set of available velocity commands, which is defined by the following constraints:

1. Speed ​​constraint: The speed range of the robot at the current moment is determined by the robot's maximum speed and acceleration and deceleration constraints: in min ≤v≤v max oh min ≤ω≤ω max (18) V min 、V max is the minimum and maximum speed that the car can reach in mechanical motion, ω min ,ω max It is the minimum and maximum angular velocity that the mechanical motion of the car can reach, which is related to the specific model of the car; 2. Dynamic constraint: In a short period of time after the current time t, that is, the dynamic window, the speed change is limited by the acceleration:

3. Obstacle constraints: within the mechanical power range of the robot, the selected trajectory cannot collide with any obstacle, and it must be able to stop or bypass it within a certain time and distance; dist(v,ω) is the distance to the obstacle calculated in the dynamic window; Select the optimal speed: (v * ,ω * ) is the selected optimal speed command, that is, the speed command with the highest score; arg max means finding the speed command that maximizes the objective function G(v,ω); (v,ω) is the set of velocity commands within the dynamic window; DW is the velocity command search space defined by considering the robot’s current state and motion capabilities, including maximum acceleration and maximum velocity; G(v,ω) is an evaluation function that evaluates the quality of the speed command based on multiple factors; (3) Combination of exploratory A* global planning algorithm and improved DWA dynamic window method: Autonomous navigation and obstacle avoidance in complex and unknown environments, organically combining the A* algorithm with the DWA dynamic window method, can significantly improve the path planning ability and real-time obstacle avoidance performance of the mobile robot. The core idea of ​​this combination strategy is to use the global planning advantage of the A* algorithm to solve the problem that the objective function solution of the DWA algorithm falls into the local optimum due to insufficient prediction length or short step length during the obstacle avoidance process. The local dynamic adaptability of the DWA algorithm solves the problem that the A* algorithm cannot consider dynamic changes in planning the global route, resulting in collisions with moving targets or suddenly intruding targets, and jointly build an efficient and safe navigation framework.

4. The self-organizing path planning and positioning technology of multiple intelligent agents based on unknown areas according to claim 1 is characterized in that: The step S3 comprises: Given the inherent randomness and complexity of unknown space exploration, especially for those closed environments that are difficult for humans to directly access or not suitable for exploration, traditional vehicle formations that rely on a single signal transmission mode face a high risk of communication interruption and positioning loss. To effectively meet this challenge, this application innovatively conceives a chain-based collaborative exploration architecture that deeply integrates the advantages of visual connection and wireless communication technology to ensure stable and reliable exploration and positioning capabilities even under extreme conditions; Specifically, the present application designs a chain exploration system consisting of multiple LiDAR navigation vehicles; these vehicles are interconnected by wireless and visual means to form a flexible and tenacious exploration chain; this design ensures that each newly added vehicle can be accurately placed within the direct monitoring range of its predecessor vehicle, which not only greatly reduces the risk of communication interruption due to signal attenuation or interference, but also realizes real-time and accurate tracking of the position of each vehicle, thereby avoiding any single node from being "lost" during the exploration process; In addition, the layout strategy of the chain structure also gives the system seamless coverage of global monitoring. Through continuous and uninterrupted line of sight connection, the entire exploration chain can maintain all-round, no-dead-angle monitoring and positioning of the surrounding environment, which is crucial for accurate navigation and path planning in complex and unknown spaces. This architecture not only improves the efficiency and safety of exploration tasks, but also provides a high-quality, high-density spatial information foundation for subsequent data analysis and spatial modeling. This function is mainly implemented in the form of judgment and constraint. It continuously judges to find situations that meet the constraint conditions and executes commands:

1. Field of view constraints: |θ L_F |<θ view (22) θ L_F =arctan(y i+1 -y i x i+1 -x i )-θ i (23) where θ L_F is the relative angle between the front robot and the rear robot, θ view is the field of view angle, i is the robot number, starting from 0 for the last robot, and so on; The above constraints ensure that the latter robot can be within the field of vision of the former robot. When the former robot is about to leave the field of vision of the latter robot, the former robot that is exploring or following will stop where it is and start monitoring the area.

2. Following distance range: d robot is the maximum distance that the robot can detect. The above formula ensures that the robot stays within the monitoring range when there are no obstacles blocking its view. Once it exceeds the range, the previous robot will stop and start monitoring the area.

3. Distribution strategy: In this project, we designed and implemented a highly coordinated robot fleet system, which explores unknown spaces in a serialized queue. The fleet is arranged in a strict long snake formation, led by a lead car, and enters the target area in turn, ensuring orderliness and safety during the journey. When the robot car at the end of the queue reaches the preset conditions or makes a decision based on real-time data analysis, it automatically or manually triggers a stop command and then switches to monitoring mode. The execution of this key action marks the full start of the internal circular judgment and constraint mechanism of the fleet. This mechanism aims to dynamically adjust the behavior strategy of each car according to the specific location of each car, environmental perception data and the exploration progress of the pilot car. Specifically, as the rear car stops and switches to monitoring mode, each car in front of it responds in turn, entering a loop judgment process driven by a preset algorithm or instant decision logic; once a car meets a specific stopping condition, it stops moving and switches to monitoring mode, continuing to provide necessary environmental information support for the overall exploration task; This process is carried out recursively until every robot car in the entire fleet stops moving and transforms into a monitoring unit according to the established strategy, or the pilot car successfully completes the exploration mission of the entire unknown space. This design not only reflects a high level of automation and intelligence, but also significantly enhances the adaptability and flexibility of the fleet when facing complex environments, providing valuable practical experience and technical support for future large-scale, high-efficiency robot exploration missions.

5. The self-organizing path planning and positioning technology of multiple intelligent agents based on unknown areas according to claim 1 is characterized in that: The step S4 comprises: S41: With itself as the origin, establish a laser coordinate system for a single laser radar, with a scanning range of 12 meters and 360 degrees. The scanning frequency is adjusted between 8-12Hz according to the actual situation; S42: according to step S31, the laser coordinate system of all laser radars is established, and the laser coordinate systems are given labels such as rplidar1, rplidar2, etc., to distinguish each laser radar; S43: measuring the distance and angle between the origin laser radar rplidar1 and another laser radar rplidar2 that it can scan, to calculate the position of rplidar2 in the world coordinate system, and similarly find the positions of all laser radars; S44: Publish static information so that the coordinate system of rplidar2 can be converted to the coordinate system of rplidar1 through TF coordinates, and all lidar coordinates are converted to the world coordinate system of rplidar1 by analogy; S45: Use Rviz on the computer to see the entire world coordinate system and the point cloud information of all lidars. The point cloud map after coordinate transformation realizes multi-perspective fusion and can observe the entire outline of the obstacle.

6. The self-organizing path planning and positioning technology of multiple intelligent agents based on unknown areas according to claim 1 is characterized in that: The step S5 comprises: The information collection capability of laser radar is used to monitor the movement trajectory of moving targets. Multi-laser radar under perspective fusion can stitch together multiple perspectives of large obstacles to achieve multi-directional and blind-angle-free joint monitoring. The information transmitted by the laser radar includes the distance d and angle θ of the polar coordinate information. According to the trigonometric function formula, the overall structure layout and the accurate coordinates of the target in the world coordinate system can be obtained; When a moving target stops in the monitoring area, the accuracy of the position is improved through the joint positioning of multiple laser radars.

Images