A robot cluster positioning motion method and system
Through wireless ad hoc network and multi-sensor fusion positioning technology, the problems of coordinated positioning of robot clusters and obstacle avoidance are solved, high-precision and stable formation movement are achieved, and the coordinated operation ability of robot clusters in complex environments is improved.
Patent Information
- Application Number
- CN202210449710.7
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2022-04-24
- Publication Date
- 2024-11-01
- Estimated Expiration
- 2042-04-24
AI Technical Summary
The existing robot cluster collaborative positioning technology has shortcomings in terms of information fusion degree, positioning accuracy, control effect and formation maintenance, and cannot effectively handle complex tasks, especially when multi-source information fusion and obstacle avoidance, it is difficult to maintain stability and Accuracy.
Use wireless ad hoc network to realize communication and dynamic networking between robots, randomly specify the pilot robot to plan the mobile path, combine satellite positioning, inertial navigation, computer vision and lidar to perform multi-sensor fusion positioning, and achieve formation changes through robot communication. To avoid obstacles, restore the original formation and continue moving.
Improve the positioning accuracy and control stability of the robot cluster, realize flexible formation changes and obstacle passage, ensure the stability and continuity of the robot cluster in complex environments, reduce the requirements of high-precision positioning of a single node, and enhance the cluster. Reliability of collaborative work.
Smart Images

Figure QLYQS_1 
Figure QLYQS_2 
Figure QLYQS_4
Abstract
Description
Technical Field
[0001] The invention relates to a robot cluster positioning movement method and system. Background Art
[0002] With the rapid development of science and technology, ground robots have been widely used in many fields such as industry and military. However, a single robot can only handle a small amount of tasks and has low work efficiency. The task volume is very limited and cannot meet the task processing needs in actual industrial or military fields. Multiple single robots are limited by the lack of coordinated movement and cannot undertake more complex tasks. Therefore, robot cluster collaboration is the future development trend.
[0003] At present, there are some technical disclosures in the field of robot cluster collaborative positioning technology. The technical methods are different, and the technical effects of various methods in terms of information fusion, positioning accuracy, control effect, formation maintenance and control between robots are also different. Each method has its own advantages and disadvantages. However, how to achieve diversified information sources between robots, higher degree of information fusion, higher positioning accuracy, better control effect, and more precise and stable formation maintenance and control is the direction for technical personnel in this field to continuously conduct in-depth research and development on this technology. In view of this, this case came into being. Summary of the invention
[0004] The present invention proposes a robot cluster positioning movement method and system, which can achieve the purposes of diversified information sources, flexible formation and maintaining formation, decentralized communication, higher positioning accuracy, more precise and stable control, and smooth obstacle avoidance.
[0005] The present invention is achieved through the following technical solutions:
[0006] A robot cluster positioning motion method, comprising:
[0007] The robots communicate with each other through wireless ad hoc networks;
[0008] Randomly designate one robot as the pilot robot from all the robots, and the rest as follower robots. Plan the movement paths of each robot with the pilot robot as the center, and control each robot to move along the movement path to form a preset formation. The movement path is related to the current position coordinates of each robot, the expected position coordinates in the preset formation, and the communication status between any two robots. If the pilot robot is lost during the movement of each robot, a new pilot robot will be randomly designated.
[0009] During the movement, the navigation and positioning information sources of each robot include satellite positioning, inertial navigation positioning, computer vision, laser radar and / or wireless positioning to control the robot to move toward the target position according to the preset formation;
[0010] During the movement, the obstacle avoidance route and the formation for passing through the obstacle area are determined according to the location of the obstacle. The formation changes are achieved through communication between robots to smoothly pass through the obstacle area, and the formation is restored after passing the obstacle to continue moving towards the target position.
[0011] Furthermore, the wireless ad hoc network is a multi-hop mobile peer-to-peer network composed of several to dozens of nodes, using wireless communication and dynamic networking.
[0012] Furthermore, the planned moving path is specifically:
[0013] The objective function of any two robots is , : , ,in, is the difference between the current horizontal coordinate and the expected horizontal coordinate of the ith robot, is the difference between the current ordinate and the expected ordinate of the ith robot, , are the communication conditions between the ith robot and the jth robot, and their values are 0 or 1. By using these two objective functions as control inputs, the horizontal coordinates / vertical coordinates of the ith robot and the jth robot can be controlled to gradually approach the horizontal coordinates / vertical coordinates of the desired positions.
[0014] Furthermore, the inertial navigation positioning is described as the following derivative equation:
[0015] ,
[0016] ,
[0017]
[0018] in, is the robot speed; is the direction cosine matrix transformed from the robot's own coordinate system to the navigation coordinate system; is the acceleration output; is the acceleration error; is the position of the robot; are the three components of the robot's velocity in terms of accuracy, latitude, and altitude; is the quaternion transformed from the robot coordinate system to the navigation coordinate system.
[0019] Furthermore, the error of the inertial navigation positioning is described as the following derivative equation:
[0020] ,in, , , , They are , , , The error, is the angular velocity transformation of the gyroscope from the inertial coordinate system to the robot coordinate system in the robot coordinate system, is the angular velocity transformation of the inertial navigation gyroscope from the inertial coordinate system to the navigation coordinate system in the navigation coordinate system, is the angle change of the robot;
[0021] The error of inertial navigation positioning will accumulate over time, and the following derivative formula is used Estimate the error state to get the accurate state of the robot, where Indicates the robot status, then , is the gyroscope error, is the system error, is the system error of the robot position , is the systematic error of gravitational acceleration , is the system error of acceleration , is the systematic error of the gyroscope, is the state transfer equation, It is the sensor error corresponding to the navigation positioning information source arriving at the current moment.
[0022] Furthermore, the covariance of the robot position corresponding to the laser radar, satellite positioning and wireless positioning Can be expressed as the following equation ,in, is the estimated robot position, is the robot position obtained directly through sensor positioning, is the point in the robot's travel map, For robots The probability of the position, , .
[0023] Furthermore, when encountering an obstacle during the traveling process, the following steps are followed to successfully pass through the obstacle area:
[0024] Step S1: Get the horizontal coordinate of the pilot robot: , the vertical axis is and the direction , calculate the relative position between the pilot robot and the obstacle as ;
[0025] Step S2, compare the relative position of the pilot robot with its detection area to determine the area where the obstacle is located. If the obstacle is located directly in front of the pilot robot, proceed to step S3. If the obstacle is located in the left front area and the right front area of the pilot robot, proceed to step S4;
[0026] Step S3, planning an obstacle avoidance route and a new formation with the pilot robot as the center, all robots realize formation changes through mutual communication to smoothly pass through the obstacle area, and restore the previous formation after passing the obstacle to continue moving toward the target position;
[0027] Step S4: Calculate the distance between obstacles in the left front area and the right front area , and calculate the reference distance ,when When , enter step S8, when When , enter step S5, when When , go to step S6;
[0028] Step S5: planning an obstacle avoidance route and a new formation with the pilot robot as the center. The formation is a row, and all robots move to avoid obstacles. After passing the obstacle, they restore the previous formation and continue to move toward the target position.
[0029] Step S6: If the obstacle is in the left front, gradually increase the angle between the leading robot and the farthest following robot on the left. , until the following calculation formula is met: , and then plan an obstacle avoidance route and a new formation with the leading robot as the center. After all robots pass the obstacle, they restore the previous formation and continue to move toward the target position;
[0030] If the obstacle is located in the front right, proceed to step S7;
[0031] Step S7: gradually reduce the angle between the pilot robot and the farthest right follower robot , until the following calculation formula is met: , and then plan an obstacle avoidance route and a new formation with the leading robot as the center. After all robots pass the obstacle, they restore the previous formation and continue to move toward the target position;
[0032] Step S8: All robots maintain their formation and pass through the obstacle directly;
[0033] in, are the coordinates of the obstacle in the left front area, are the coordinates of the obstacle in the right front area, is the radius of the robot, is the maximum measurement value of the robot's sensors, including computer vision and lidar. A is the pre-set safety threshold for the robot's current formation. is the ordinate of the robot farthest from the left, is the distance between the leading robot and the farthest following robot on the left. is the ordinate of the farthest following robot on the right, It is the distance between the leading robot and the farthest following robot on the right.
[0034] Furthermore, the formation control process of the following robot during the march is:
[0035] The transfer equations of each follower robot are: ,
[0036] The state vector of a single follower robot is ,
[0037] in, For the Follower Robot Moment Axis coordinates, For the Follower Robot The speed of time, For the Follower Robot The angular velocity at time, For the Follower Robot The direction of the moment,
[0038] The observation equation of each follower robot is: , ,
[0039] in,[ ] is the estimation of the position, velocity, angular velocity and orientation of each random robot in turn, [ ] are the observed values of the position, velocity, angular velocity and orientation of each follower robot in turn. is the state observation of each follower robot, To perform tf transformation;
[0040] The motion calculation equations for positioning and adjusting the position of each follower robot are as follows:
[0041] Kinematic equations: ,
[0042] in, For the system Always follow the estimate of the robot's state vector, for Always follow the robot state vector, for The team's motion state at the moment;
[0043] Poor defensive transfer: ,
[0044] in, is the Jacobian matrix, for The covariance of the time, for Confidence at the moment;
[0045] Actual observed quantity: ,
[0046] in, for The observation amount of the leading robot to the following robot at all times, for Always follow the robot's own observation estimation transformation,
[0047] Observation covariance: ,
[0048] Gains: ,
[0049] in, is the transfer matrix, is the covariance transfer, For noise;
[0050] Update equation: ,
[0051] in, for The robot state vector at the moment, For the system Estimation of the robot state vector at the moment;
[0052] Update covariance transfer: ,
[0053] Where I is the identity matrix;
[0054] Observation Jacobian matrix: , .
[0055] The present invention is also achieved through the following technical solutions:
[0056] A robot cluster positioning motion system, comprising a plurality of robots and:
[0057] Communication module: including wireless ad hoc network, used to enable communication between robots;
[0058] Preset formation determination module: used to randomly designate one robot as the pilot robot from all robots, and the rest as follower robots. The moving paths of each robot are planned with the pilot robot as the center, and each robot is controlled to move along the moving path to form a preset formation. The moving path is related to the current position coordinates of each robot, the expected position coordinates in the preset formation, and the communication status between any two robots. During the movement of each robot, if the pilot robot is lost, the pilot robot will be randomly re-designated;
[0059] Navigation and positioning module: including satellite positioning, inertial navigation positioning, computer vision, lidar and / or wireless positioning, which uses these positioning data as input to control the robot to move toward the target position according to the preset formation;
[0060] Obstacle avoidance module: used to determine the obstacle avoidance route and the formation for passing through the obstacle area according to the location of the obstacle. The formation changes are achieved through communication between robots so as to pass through the obstacle area smoothly. After passing the obstacle, the formation is restored and the robots continue to move towards the target position.
[0061] The present invention has the following beneficial effects:
[0062] 1. The robots of the present invention communicate with each other through a wireless self-organizing network, realizing decentralized control and dynamic networking, and effectively improving network reliability; when forming a formation, a pilot-following method is adopted to coordinate the formation, a pilot robot is randomly designated, and the moving paths of each robot are planned with the pilot robot as the center, and each robot is controlled to move according to the moving path to form a preset formation, thereby ensuring that the robots move without collision during the formation process and eventually reach the target position, and can also avoid the problem of loss of formation due to loss of the pilot robot during the march, thereby preventing the problem of continued collaborative positioning; multi-sensor positioning fusion positioning is adopted for positioning, which can be as far as possible during the march Taking advantage of the positioning capabilities of each sensor and complementing each other in different scenarios, the impact of cumulative errors can be reduced or even eliminated, thereby improving the positioning accuracy of robot clusters in weak signal areas. This reduces the functional requirements for each robot in the cluster, while increasing the reliability and stability of cluster collaborative operations, solving the problem of high-precision horizontal and vertical positioning of a single node, and maintaining the stability and continuity of robot cluster movement in various complex situations. The lead robot and follower robot can coordinate with each other in obstacle avoidance, and the follower robot can adjust its relative position with the lead robot in real time according to environmental constraints, achieving flexible formation changes, allowing the entire robot cluster to pass through obstacle areas without collision. DETAILED DESCRIPTION
[0063] The robot cluster positioning motion system includes multiple robots, a communication module, a preset formation module, a navigation and positioning module and an obstacle avoidance module. Each robot sets its own number. The communication module includes a wireless self-organizing network for communication between robots. The preset formation module is used to randomly designate one robot as a pilot robot from all robots, and the rest are follower robots. The moving path of each robot is planned with the pilot robot as the center, and each robot is controlled to move along the moving path to form a preset formation. The moving path is related to the current position coordinates of each robot, the expected position coordinates in the preset formation and the communication status between any two robots. During the movement of each robot, if the pilot robot is lost, the pilot robot is randomly re-designated. The navigation and positioning module includes satellite positioning, inertial navigation positioning, computer vision, laser radar and wireless positioning, and these positioning data are used as input to control the robot to move toward the target position according to the preset formation. The obstacle avoidance module is used to determine the obstacle avoidance route and the formation formation through the obstacle area according to the position of the obstacle, and realize the change of the formation through the communication between the robots to smoothly pass through the obstacle area, and restore the formation formation after passing the obstacle to continue moving to the target position.
[0064] One of the key technologies for collaborative control of cluster robots is communication technology. Since WiFi wireless communication technology has many advantages and can meet the needs of self-organizing cluster robot communication systems, this embodiment uses a wireless communication system solution. The quality of wireless communication directly affects the collaborative effect between cluster robots. In the cluster robot system, the communication system must be stable and reliable, and can achieve rapid information interaction, and build a wireless communication platform for self-organizing cluster robots. The robots in this embodiment use a wireless ad hoc network for communication. The wireless ad hoc network is a multi-hop mobile peer-to-peer network composed of several to dozens of nodes, using wireless communication methods, and dynamic networking. More specifically, it is an Ad-Hoc network. This network transmits voice streams with service quality requirements through dynamic routing and mobile management technology. Usually, the nodes have a continuous energy supply and do not need fixed equipment support. Each node is networked by itself. During communication, other user nodes forward data. This network form breaks through the geographical limitations of traditional wireless networks and can be deployed more quickly, conveniently, and efficiently. It is suitable for scenes where there is no network on site and a fast network construction and convenient installation are required. Ad-Hoc network has no strict control center, all nodes are equal, that is, it is a peer-to-peer network, nodes can join and leave the network at any time, any node failure will not affect the operation of the entire network, with strong anti-destruction ability; the layout or deployment of the network does not need to rely on any preset network facilities, nodes coordinate their own behavior through layered protocols and distributed algorithms, and nodes can quickly and automatically form an independent network after being turned on; when a node wants to communicate with nodes outside its coverage range, multi-hop forwarding of intermediate nodes is required. Different from the multi-hop of fixed networks, multi-hop routing in Ad-Hoc networks is completed by ordinary network nodes, not by dedicated routing devices (such as routers); Ad-Hoc network is a dynamic network, network nodes can move anywhere, and can be turned on and off at any time, which will cause the network topology to change at any time.
[0065] In order for the robot cluster to reach the target position, a preset formation is first required (the specific process of this step is the existing technology). In order for each robot to quickly reach its own position in the preset formation, the system treats all robots as known landmarks and controls them in a pilot-following method. The pilot-following method is specifically to randomly designate one robot as the pilot robot from all robots, and the rest as follower robots. The moving paths of each robot are planned with the pilot robot as the center, and each robot is controlled to move along the moving path to form a preset formation. The moving path is related to the current position coordinates of each robot, the expected position coordinates in the preset formation, and the communication between any two robots. During the movement of each robot, if the pilot robot is lost, the pilot robot is randomly re-designated. The loss of the pilot robot refers to a special situation caused by an accident, such as robot damage, communication interruption, and navigation failure. After the pilot robot is lost, the pilot robot is randomly re-designated, and the moving paths of other robots do not change.
[0066] The planned moving path is as follows:
[0067] set up is the current horizontal coordinate of the ith robot, is the expected horizontal coordinate of the ith robot, which is the horizontal coordinate of the robot's position in the preset formation. is the difference between the current horizontal coordinate and the expected horizontal coordinate of the ith robot. According to the consistency algorithm, the objective function of any two robots is obtained. for:
[0068] ,
[0069] Among them, the consistency algorithm is a known fixed algorithm. Consistency is a key issue in robot cluster formation, which means that all robots in the cluster have the same state within a period of time;
[0070] is the communication between the ith robot and the jth robot. If the ith robot can obtain the information of the jth robot, then ,otherwise, ;
[0071] make , is the difference between the expected horizontal coordinates of the ith robot and the jth robot, then the above objective function is , using the objective function as the control input, the horizontal coordinates of the i-th robot and the j-th robot can be controlled to gradually approach the horizontal coordinates of the desired position.
[0072] By analogy, let is the current ordinate of the ith robot, is the expected ordinate of the ith robot, which is the ordinate of the robot's position in the preset formation. is the difference between the current ordinate and the expected ordinate of the i-th robot. According to the consistency algorithm, the objective function of any two robots is obtained. for:
[0073] ,
[0074] is the communication between the ith robot and the jth robot. If the ith robot can obtain the information of the jth robot, then =1, otherwise, =0;
[0075] make , is the difference between the expected ordinates of the ith robot and the jth robot, then the above objective function is , taking the objective function as the control input, the vertical coordinates of the i-th robot and the j-th robot can be controlled to gradually approach the vertical coordinates of the desired position;
[0076] This will form a preset formation.
[0077] During the movement, the navigation and positioning information sources of each robot include satellite positioning (GNSS-Beidou positioning), inertial navigation positioning (IMU), computer vision (CV), laser radar (LIDAR) and wireless positioning to control the robot to move towards the target position according to the preset formation;
[0078] Specifically:
[0079] Use east-north-up (ENU) as the navigation coordinate system , using right-forward-up (RFU) as the coordinate system of each robot , the earth coordinate system is expressed as And the inertial coordinate system is expressed as ;
[0080] In the navigation coordinate system Inertial navigation positioning is described as the following derivative equation:
[0081] ,
[0082] ,
[0083] ,
[0084] in, is the robot speed; From the robot's own coordinate system Transform to navigation coordinate system The direction cosine matrix of is the acceleration output; is the acceleration error; In the navigation coordinate system From the inertial coordinate system To Earth Coordinate System The angular velocity transformation, In the navigation coordinate system From the Earth coordinate system To the navigation coordinate system The angular velocity transformation, In the robot coordinate system From the navigation coordinate system To the robot coordinate system Angular velocity transformation; is the acceleration due to gravity, is the position of the robot, is the longitude, is the latitude, is the height, is the quaternion multiplication symbol; are the three components of the robot's velocity in longitude, latitude and altitude, and their expressions are: , in this expression, and are the lateral radius and the latitudinal radius respectively, From the robot coordinate system Transform to navigation coordinate system The quaternion of To find the derivative;
[0085] The error of inertial navigation positioning is described by the following derivative equation:
[0086] ,in, , , , They are , , , The error, The IMU gyroscope is in the robot coordinate system From the inertial coordinate system To the robot coordinate system The angular velocity transformation, The gyroscope for inertial navigation is in the navigation coordinate system From the inertial coordinate system To the navigation coordinate system The angular velocity transformation, is the angle change of the robot;
[0087] Since the error of inertial navigation accumulates over time, in order to obtain the accurate state of each robot, Kalman filter estimation is used to estimate the error state and update the system output:
[0088] Use the following derivation formula Estimate the error state to get the accurate state of the robot, where Indicates the robot status, then , is the gyroscope error, is the system error, is the system error of the robot position , is the systematic error of gravitational acceleration , is the system error of acceleration , is the systematic error of the gyroscope, is the state transfer equation, It is the sensor error corresponding to the navigation and positioning information source arriving at the current moment, such as lidar and GNSS.
[0089] Laser radar, satellite positioning, computer vision and wireless positioning are global positioning methods, that is, the system can directly obtain the global position through these sensor positioning, so for these four sensor positioning, the system update equation can be uniformly expressed, and the sensor input of the entire system continuously updates the error to control the error within a range. For the input of laser radar, satellite positioning and wireless positioning, the covariance of the corresponding robot position is Can be expressed as the following equation ,in, is the estimated robot position, is the robot position obtained directly through sensor positioning, are the horizontal and vertical coordinates of the points in the robot's travel map, For robots The probability of the position, the estimated value expression of x, y is: , .
[0090] The formation control process of the following robot during movement is as follows:
[0091] The transfer equations of each follower robot are:
[0092] ,
[0093] The state vector of a single follower robot is ,
[0094] in, For the Follower Robot Moment Axis coordinates, For the Follower Robot The speed of time, For the Follower Robot The angular velocity at time, For the Follower Robot The direction of the moment,
[0095] The observation equation of each follower robot is: , ,
[0096] in,[ ] is the estimation of the position, velocity, angular velocity and orientation of each random robot in turn, [ ] are the observed values of the position, velocity, angular velocity and orientation of each follower robot in turn. is the state observation of each follower robot, To perform tf transformation, Perform tf transformation and get ;
[0097] The motion calculation equations for positioning and adjusting the position of each follower robot are as follows:
[0098] Kinematic equations: ,
[0099] in, For the system Always follow the estimate of the robot's state vector, for Always follow the robot state vector, for The team's motion state at the moment;
[0100] Poor defensive transfer: ,
[0101] in, is the Jacobian matrix, for The covariance of time, for The confidence level at the moment, the acquisition process of which is the existing technology;
[0102] Actual observed quantity: ,
[0103] in, for The observation amount of the leading robot to the following robot at all times, for Always follow the robot's own observation estimation transformation,
[0104] Observation covariance: ,
[0105] Gains: ,
[0106] in, is the transfer matrix, is the covariance transfer, For noise;
[0107] Update equation: ,
[0108] in, for The robot state vector at the moment, For the system Estimation of the robot state vector at the moment;
[0109] Update covariance transfer: ,
[0110] Where I is the identity matrix;
[0111] Observation Jacobian matrix: , .
[0112] During the implementation process, a variety of sensor fusion positioning technologies are used to maximize the advantages of each sensor and complement the scene, reducing or even eliminating the impact of cumulative errors. The leading robot can observe the positions of all other robots through sensing, while the following robot can observe the robots near it. The following robot is positioned through the EKF (Kalman filter) fusion method. The observation of the following robot always appears at the fixed position of the robot, and the numbers of other observed robots are also fixed. The leading robot uses confidence to describe the position of each robot. Each robot adjusts its global position (which can be the position in the map) based on the confidence and combined with the observation. That is, through the method of prediction by the leading robot and correction by the following robot, the overall positioning accuracy of the formation is improved to achieve continuous and reliable positioning results.
[0113] During the process of moving, the obstacle avoidance route and the formation for passing through the obstacle area are determined according to the position of the obstacle. The formation change is realized through direct or indirect communication between robots to smoothly pass through the obstacle area, and the formation is restored after passing the obstacle to continue moving to the target position. The specific steps include:
[0114] Step S1: Obtain the horizontal coordinate of the pilot robot through observation: , the vertical axis is and the direction , calculate the relative position between the pilot robot and the obstacle as ;
[0115] Step S2, compare the relative position of the pilot robot with the value range of the detection area of its sensor to determine the area where the obstacle is located, and the specific comparison process is the prior art; if the obstacle is located in front of the pilot robot, proceed to step S3, if the obstacle is located in the left front area and the right front area of the pilot robot, proceed to step S4;
[0116] Step S3, planning an obstacle avoidance route and a new formation with the pilot robot as the center, all robots realize formation changes through mutual communication, so as to smoothly pass through the obstacle area, and restore the previous formation after passing the obstacle to continue moving towards the target position; wherein, the process of planning the obstacle avoidance route and the new formation is an existing technology;
[0117] Step S4: Calculate the distance between obstacles in the left front area and the right front area , and calculate the reference distance ,when When , enter step S8, when When , enter step S5, when When , go to step S6; are the coordinates of the obstacle in the left front area, is the coordinate of the obstacle in the right front area; is the radius of the robot, is the maximum measurement value of the robot's sensors, including computer vision and lidar, and A is the pre-set safety threshold for the robot's current formation
[0118] Step S5, planning an obstacle avoidance route and a new formation with the pilot robot as the center, the formation is that all robots move and transform into a line, then all robots move and avoid obstacles according to the obstacle avoidance route, and restore the previous formation after passing the obstacle and continue to move towards the target position;
[0119] Step S6: If the obstacle is in the left front, gradually increase the angle between the leading robot and the farthest following robot on the left. , until the following calculation formula is met: , and then plan an obstacle avoidance route and a new formation with the pilot robot as the center. All robots transform into the new formation and move according to the obstacle avoidance route. After all robots pass the obstacle, they restore the previous formation and continue to move towards the target position; is the ordinate of the robot farthest from the left, The distance between the leading robot and the farthest following robot on the left;
[0120] If the obstacle is located in the front right, proceed to step S7;
[0121] Step S7: gradually reduce the angle between the pilot robot and the farthest right follower robot , until the following calculation formula is met: , and then plan the obstacle avoidance route and new formation with the leading robot as the center. After all robots pass the obstacle, they resume the previous formation and continue to move towards the target position; is the ordinate of the farthest following robot on the right, The distance between the leading robot and the farthest following robot on the right;
[0122] Step S8: All robots maintain their formation and pass through the obstacle directly.
[0123] The lead robot and the follower robot coordinate with each other in obstacle avoidance. The follower robot adjusts its relative position with the lead robot in real time according to environmental constraints to achieve flexible formation defense, so that the entire robot cluster can pass through the obstacle area without collision.
[0124] The above description is only a preferred embodiment of the present invention, and therefore cannot be used to limit the scope of implementation of the present invention. That is, equivalent changes and modifications made according to the scope of the patent application of the present invention and the contents of the specification should still fall within the scope covered by the patent of the present invention.
Claims
1. A robot cluster positioning motion method, characterized in that: include: The robots communicate with each other through wireless ad hoc networks; Randomly designate one robot as the pilot robot from all the robots, and the rest as follower robots. Plan the movement paths of each robot with the pilot robot as the center, and control each robot to move along the movement path to form a preset formation. The movement path is related to the current position coordinates of each robot, the expected position coordinates in the preset formation, and the communication status between any two robots. If the pilot robot is lost during the movement of each robot, a new pilot robot will be randomly designated. During the movement, the navigation and positioning information sources of each robot include satellite positioning, inertial navigation positioning, computer vision, laser radar and / or wireless positioning to control the robot to move toward the target position according to the preset formation; During the movement, the obstacle avoidance route and the formation for passing through the obstacle area are determined according to the location of the obstacle. The formation is changed through communication between robots to smoothly pass through the obstacle area, and the formation is restored after passing the obstacle to continue moving towards the target position. The wireless ad hoc network is a multi-hop mobile peer-to-peer network consisting of several to dozens of nodes, using wireless communication and dynamic networking; The planned moving path is specifically: The objective function of any two robots is Among them, z i (t) is the difference between the current horizontal coordinate and the expected horizontal coordinate of the ith robot, is the difference between the current ordinate and the expected ordinate of the ith robot, a ij , ij are the communication conditions between the ith robot and the jth robot, and the values are 0 or 1. By using these two objective functions as control inputs, the horizontal coordinates / vertical coordinates of the ith robot and the jth robot can be controlled to gradually approach the horizontal coordinates / vertical coordinates of the desired positions. When encountering an obstacle during the traveling process, the following steps are followed to successfully pass through the obstacle area: Step S1: Get the horizontal coordinate of the pilot robot: The vertical axis is and the direction Calculate the relative position between the pilot robot and the obstacle: Step S2, comparing the relative position of the pilot robot with its detection area to determine the area where the obstacle is located. If the obstacle is located directly in front of the pilot robot, proceed to step S3; if the obstacle is located in the left front area and the right front area of the pilot robot, proceed to step S4; Step S3, planning an obstacle avoidance route and a new formation with the pilot robot as the center, all robots realize formation changes through mutual communication to smoothly pass through the obstacle area, and restore the previous formation after passing the obstacle to continue moving toward the target position; Step S4: Calculate the distance between obstacles in the left front area and the right front area And calculate the reference distance l d = l ob -(4d r +3d s ), when l d ≥A, go to step S8, when l d <0, go to step S5, when 0<l d When <A, go to step S6; Step S5: planning an obstacle avoidance route and a new formation with the pilot robot as the center. The formation is a row, and all robots move to avoid obstacles. After passing the obstacle, they restore the previous formation and continue to move toward the target position. Step S6: If the obstacle is in the left front, gradually increase the angle between the leading robot and the farthest following robot on the left. Until the following calculation formula is met: Then, the leading robot is used as the center to plan an obstacle avoidance route and a new formation. After all robots pass the obstacle, they resume their previous formation and continue to move toward the target location. If the obstacle is located in the front right, proceed to step S7; Step S7: gradually reduce the angle between the pilot robot and the farthest right follower robot Until the following calculation formula is met: Then, the leading robot is used as the center to plan an obstacle avoidance route and a new formation. After all robots pass the obstacle, they resume their previous formation and continue to move toward the target location. Step S8: All robots maintain their formation and pass through the obstacle directly; in, are the coordinates of the obstacle in the left front area, is the coordinate of the obstacle in the right front area, d r. is the radius of the robot, d s. is the maximum measurement value of the robot's sensors, including computer vision and lidar. A is the pre-set safety threshold for the robot's current formation. is the ordinate of the farthest follower robot on the left, l1 is the distance between the leading robot and the farthest follower robot on the left, is the ordinate of the farthest following robot on the right, l 2. The distance between the leading robot and the farthest following robot on the right; The formation control process of the following robot during movement is as follows: The transfer equations of each follower robot are: The state vector of a single follower robot is X i,t =[x i,t ,y i,t , v i,t ,ω i,t ,θ i,t ] T , Among them, x i,t ,y i,t is the x- and y-axis coordinates of the ith following robot at time t, v i,t: is the speed of the ith following robot at time t, ω i,t: is the angular velocity of the i-th following robot at time t, θ i,t is the orientation of the i-th following robot at time t, The observation equation of each follower robot is: X i,t =X i,obj,t +tf i,x,t , in, To estimate the position, velocity, angular velocity and orientation of each random robot in turn, [x i,o,t y i,o,t v i,o,t ω i,o,t θ i,o,t ] is the observed value of the position, velocity, angular velocity and orientation of each follower robot in turn, X i,obj,t is the state observation of each following robot, tf i,x,t To perform tf transformation; The motion calculation equations for positioning and adjusting the position of each follower robot are as follows: Kinematic equations: in, is the system’s estimate of the state vector of the following robot at time t, X t-1. is the state vector of the robot at time t-1, u t. is the team motion state at time t; Poor defensive transfer: Among them, J t-1 is the Jacobian matrix, P t-1 / t-1 is the covariance at time t-1, Q t is the confidence level at time t; Actual observed quantity: Among them, z t is the observation of the leader robot on the follower robot at time t, is the estimated transformation of the observation quantity following the robot itself at time t, Observation covariance: Gains: Among them, H t is the transfer matrix, P t / t-1 is the covariance transfer, R t for noise; Update equation: Among them, X t. is the robot state vector at time t, is the system's estimate of the robot's state vector at time t; Update covariance transfer: P t / t =(IK t H t ) t / t-1 , Where I is the identity matrix; Observation Jacobian matrix:
2. A robot cluster positioning motion method according to claim 1, characterized in that: The inertial navigation positioning is described as the following derivative equation: Among them, v n is the robot speed; is the direction cosine matrix from the robot's own coordinate system to the navigation coordinate system; f b is the acceleration output; b a is the acceleration error; r is the position of the robot; R c are the three components of the robot's velocity in terms of accuracy, latitude, and altitude; is the quaternion transformed from the robot coordinate system to the navigation coordinate system.
3. A robot cluster positioning motion method according to claim 2, characterized in that: The error of the inertial navigation positioning is described as the following derivative equation: Among them, δv n ,δr,δf b , They are v n , r, f b , The error, is the angular velocity transformation of the gyroscope from the inertial coordinate system to the robot coordinate system in the robot coordinate system, is the angular velocity transformation of the inertial navigation gyroscope from the inertial coordinate system to the navigation coordinate system in the navigation coordinate system, and ψ is the angle change of the robot; The error of inertial navigation positioning will accumulate over time, and the following derivative formula is used Estimate the error state to get the accurate state of the robot, where Indicates the robot status, then b g is the gyroscope error, is the system error, W a. is the system error of the robot position, W g is the systematic error of gravitational acceleration, is the systematic error of acceleration, is the system error of the gyroscope, F(X) is the state transfer equation, and G(X) is the sensor error corresponding to the navigation positioning information source arriving at the current moment.
4. A robot cluster positioning motion method according to claim 1, 2 or 3, characterized in that: The covariance c of the robot position corresponding to the laser radar, satellite positioning and wireless positioning xy Can be expressed as the following equation in, is the estimated robot position, is the robot position obtained directly through sensor positioning, (x, y) is the point in the robot's travel map, and P(x, y) β is the probability that the robot is at position (x, y), 5. A robot cluster positioning motion system, characterized in that: Includes multiple robots and: Communication module: including wireless ad hoc network, used to enable communication between robots; Preset formation determination module: used to randomly designate one robot as the pilot robot from all robots, and the rest as follower robots. The moving paths of each robot are planned with the pilot robot as the center, and each robot is controlled to move along the moving path to form a preset formation. The moving path is related to the current position coordinates of each robot, the expected position coordinates in the preset formation, and the communication status between any two robots. During the movement of each robot, if the pilot robot is lost, the pilot robot will be randomly re-designated; Navigation and positioning module: including satellite positioning, inertial navigation positioning, computer vision, lidar and / or wireless positioning, which uses these positioning data as input to control the robot to move toward the target position according to the preset formation; Obstacle avoidance module: used to determine the obstacle avoidance route and the formation for passing through the obstacle area according to the location of the obstacle. The formation is changed through communication between robots to smoothly pass through the obstacle area, and the formation is restored after passing the obstacle to continue moving towards the target position. The wireless ad hoc network is a multi-hop mobile peer-to-peer network consisting of several to dozens of nodes, using wireless communication and dynamic networking; The planned moving path is specifically: The objective function of any two robots is Among them, z i (t) is the difference between the current horizontal coordinate and the expected horizontal coordinate of the ith robot, is the difference between the current ordinate and the expected ordinate of the ith robot, a ij , ij are the communication conditions between the ith robot and the jth robot, and the values are 0 or 1. By using these two objective functions as control inputs, the horizontal coordinates / vertical coordinates of the ith robot and the jth robot can be controlled to gradually approach the horizontal coordinates / vertical coordinates of the desired positions. When encountering obstacles during the journey, follow the steps below to successfully pass through the obstacle area: Step S1: Get the horizontal coordinate of the pilot robot: The vertical axis is and the direction Calculate the relative position between the pilot robot and the obstacle: Step S2, comparing the relative position of the pilot robot with its detection area to determine the area where the obstacle is located. If the obstacle is located directly in front of the pilot robot, proceed to step S3; if the obstacle is located in the left front area and the right front area of the pilot robot, proceed to step S4; Step S3, planning an obstacle avoidance route and a new formation with the pilot robot as the center, all robots realize formation changes through mutual communication to smoothly pass through the obstacle area, and restore the previous formation after passing the obstacle to continue moving toward the target position; Step S4: Calculate the distance between obstacles in the left front area and the right front area And calculate the reference distance l d = l ob -(4d r +3d s ), when l d ≥A, go to step S8, when l d <0, go to step S5, when 0<l d When <A, go to step S6; Step S5: planning an obstacle avoidance route and a new formation with the pilot robot as the center. The formation is a row, and all robots move to avoid obstacles. After passing the obstacle, they restore the previous formation and continue to move toward the target position. Step S6: If the obstacle is in the left front, gradually increase the angle between the leading robot and the farthest following robot on the left. Until the following calculation formula is met: Then, the leading robot is used as the center to plan an obstacle avoidance route and a new formation. After all robots pass the obstacle, they resume their previous formation and continue to move toward the target location. If the obstacle is located in the front right, proceed to step S7; Step S7: gradually reduce the angle between the pilot robot and the farthest right follower robot Until the following calculation formula is met: Then, the leading robot is used as the center to plan an obstacle avoidance route and a new formation. After all robots pass the obstacle, they resume their previous formation and continue to move toward the target location. Step S8: All robots maintain their formation and pass through the obstacle directly; in, are the coordinates of the obstacle in the left front area, is the coordinate of the obstacle in the right front area, d r. is the radius of the robot, d s. is the maximum measurement value of the robot's sensors, including computer vision and lidar. A is the pre-set safety threshold for the robot's current formation. is the ordinate of the farthest follower robot on the left, l1 is the distance between the leading robot and the farthest follower robot on the left, is the ordinate of the farthest following robot on the right, l2 is the distance between the leading robot and the farthest following robot on the right; The formation control process of the following robot during movement is as follows: The transfer equations of each follower robot are: The state vector of a single follower robot is X i,t =[x i,t ,y i,t , v i,t ,ω i,t ,θ i,t ] T , Among them, x i,t ,y i,t is the x- and y-axis coordinates of the ith following robot at time t, v i,t is the speed of the ith following robot at time t, ω i,t is the angular velocity of the i-th following robot at time t, θ i,t is the orientation of the i-th following robot at time t, The observation equation of each follower robot is: , in, To estimate the position, velocity, angular velocity and orientation of each random robot in turn, [x i,o,t y i,o,t v i,o,t ω i,o,t θ i,o,t ] is the observed value of the position, velocity, angular velocity and orientation of each follower robot in turn, X i,obj,t is the state observation of each following robot, tf i,x,t To perform tf transformation; The motion calculation equations for positioning and adjusting the position of each follower robot are as follows: Kinematic equations: in, is the system’s estimate of the state vector of the following robot at time t, X t-1 is the state vector of the robot at time t-1, u t. is the team motion state at time t; Poor defensive transfer: Among them, J t-1 is the Jacobian matrix, P t-1 / t-1 is the covariance at time t-1, Q t is the confidence level at time t; Actual observed quantity: Among them, z t is the observation of the leader robot on the follower robot at time t, is the estimated transformation of the observation quantity following the robot itself at time t, Observation covariance: Gains: Among them, H t is the transfer matrix, P t / t-1 is the covariance transfer, R t for noise; Update equation: Among them, X t is the robot state vector at time t, is the system's estimate of the robot's state vector at time t; Update covariance transfer: P t / t =(IK t H t ) t / t-1 , Where I is the identity matrix; Observation Jacobian matrix:
