CN114326731B - Multi-robot formation tracking sampling control method and system based on laser radar - Google Patents
Multi-robot formation tracking sampling control method and system based on laser radar Download PDFInfo
- Publication number
- CN114326731B CN114326731B CN202111626053.0A CN202111626053A CN114326731B CN 114326731 B CN114326731 B CN 114326731B CN 202111626053 A CN202111626053 A CN 202111626053A CN 114326731 B CN114326731 B CN 114326731B
- Authority
- CN
- China
- Prior art keywords
- robot
- formation
- speed
- target
- distance
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 230000015572 biosynthetic process Effects 0.000 title claims abstract description 154
- 238000000034 method Methods 0.000 title claims abstract description 37
- 238000005070 sampling Methods 0.000 title claims abstract description 33
- 238000005259 measurement Methods 0.000 claims description 18
- 230000033001 locomotion Effects 0.000 claims description 17
- 239000011159 matrix material Substances 0.000 claims description 10
- 238000006073 displacement reaction Methods 0.000 claims description 9
- 230000009466 transformation Effects 0.000 claims description 3
- 238000005755 formation reaction Methods 0.000 description 110
- 238000004891 communication Methods 0.000 description 4
- 238000010586 diagram Methods 0.000 description 4
- 230000008859 change Effects 0.000 description 2
- 230000007547 defect Effects 0.000 description 2
- 238000011161 development Methods 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 230000006872 improvement Effects 0.000 description 2
- 238000013507 mapping Methods 0.000 description 2
- 230000009012 visual motion Effects 0.000 description 2
- 238000013459 approach Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000009954 braiding Methods 0.000 description 1
- 239000003795 chemical substances by application Substances 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 230000008030 elimination Effects 0.000 description 1
- 238000003379 elimination reaction Methods 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 230000006870 function Effects 0.000 description 1
- 230000004807 localization Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000012544 monitoring process Methods 0.000 description 1
- 230000008447 perception Effects 0.000 description 1
- 230000011273 social behavior Effects 0.000 description 1
- 239000013598 vector Substances 0.000 description 1
- 230000000007 visual effect Effects 0.000 description 1
Classifications
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02P—CLIMATE CHANGE MITIGATION TECHNOLOGIES IN THE PRODUCTION OR PROCESSING OF GOODS
- Y02P90/00—Enabling technologies with a potential contribution to greenhouse gas [GHG] emissions mitigation
- Y02P90/02—Total factory control, e.g. smart factories, flexible manufacturing systems [FMS] or integrated manufacturing systems [IMS]
Landscapes
- Manipulator (AREA)
Abstract
The invention discloses a laser radar-based multi-robot formation tracking sampling control method and a laser radar-based multi-robot formation tracking sampling control system, which belong to the field of intelligent robot control, wherein the method comprises the following steps: each robot respectively measures the distance between the robot and the adjacent robot at the current time, and calculates the first speed of the robot under the body coordinate system by taking the formation shape of the plurality of robots approaching to the expected formation shape as a target; when the shape of the multi-robot formation is consistent with the expected formation shape, the pilot robot calculates the target speed of the multi-robot formation under the robot formation coordinate system by using a PI control method according to the current pose, the target speed and the target pose of the multi-robot formation, and sends the target speed to other robots; each robot takes the formation speed of multiple robots as a target, calculates the second speed of the robots under the body coordinate system, controls the speed of the robots from the current moment to the next moment to be equal to the combined speed of the first speed and the second speed, and realizes formation tracking control.
Description
Technical Field
The invention belongs to the field of intelligent control of robots, and particularly relates to a multi-robot formation tracking and sampling control method and system based on a laser radar.
Background
The multi-robot system has been increasingly used in the fields of environmental monitoring, rescue, assistance in transportation and the like due to the advantages of strong feasibility, low communication cost, strong robustness and the like. As a basis for these applications, multi-robot formation tracking control becomes a critical issue. In the existing multi-robot formation tracking control technology, two formation tracking control methods based on position and distance are mainly adopted according to the requirement of perception capability.
The position-based formation tracking control method requires that all robots have complete global positioning information, and in practical application, only outdoor real-time dynamic differential positioning (Real Time Kinematic, RTK) technology and an indoor visual motion capture system can ensure the effect, but the equipment has the defects of high price, difficult deployment and the like, and the system cost is increased.
The distance-based formation tracking control method requires only a part of robots to have global position information and target information, but has the following disadvantages: typically, a continuous time model is used, and such controllers cannot be used directly in a real system; the laser radar serving as a common distance sensor cannot directly identify the identities of the neighbors, and the measurement error of the laser radar causes inconsistent distance measurement between the neighbors, so that formation distortion and unexpected circular motion are caused; an agent without target information calculates the control output of the next step through observing the neighbor state, which often leads to formation distortion when the target motion state is suddenly changed. Therefore, how to provide a more low-cost, accurate and practical multi-robot formation tracking control method has important significance.
Disclosure of Invention
Aiming at the defects and improvement demands of the prior art, the invention provides a multi-robot formation tracking sampling control method and system based on a laser radar, which aim to solve the problems that continuous measurement and communication are needed during the formation control of the prior robot, measurement is inconsistent due to measurement errors, and formation distortion is caused during the target tracking.
To achieve the above object, according to one aspect of the present invention, there is provided a multi-robot formation tracking sampling control method based on a laser radar, including: s1, each robot respectively measures the distance between the robot and the adjacent robot at the current moment, takes the formation shape of the multiple robots as a target, and calculates the first speed of the robot under the body coordinate system according to the measured distance; s2, when the shape of the multi-robot formation is consistent with the expected formation shape, the pilot robot in the multi-robot calculates the target speed of the multi-robot formation under a robot formation coordinate system according to the current pose, the target speed and the target pose of the multi-robot formation by using a PI control method, and sends the target speed to other robots in the multi-robot; s3, each robot takes the formation speed of multiple robots as a target, calculates a second speed of the robot under the body coordinate system according to the measured distance, calculates a combined speed of the first speed and the second speed, and controls the speed of the robot from the current moment to the next moment to be equal to the combined speed.
Still further, when the multi-robot formation shape is not consistent with the expected formation shape, the steps between S1 and S3 include: the pilot robot sets the target speed of the multi-robot formation in the robot formation coordinate system to 0 and sends the target speed to other robots.
Further, the time difference T between two adjacent moments is:
wherein sigma is the preset maximum displacement of the robot between two adjacent moments, v max Is the maximum movement speed, k of the robot c First control gain, lambda, for multi-robot formation max (L) is the maximum eigenvalue of L, L is the Laplacian matrix of the topology graph G describing the robot neighbor relation.
Further, the first speed of each robot in its body coordinate system is:
wherein, i v′ x,i (k)、 i v′ y,i (k) The velocity components of the first velocity of the robot i in the x and y directions under the body coordinate system at the moment k, i x j (k)、 i y j (k) The coordinates of the robot j in the x and y directions under the coordinate system of the robot i at the moment k are respectively, and d (i,j) (k) For the measured distance between robot i and robot j at time k, N i K is the set of robots adjacent to robot i c Formation of multiple robotsA control gain D (i,j) For a desired distance between robot i and robot j,an estimated value of the difference between the distance measurement values of robot i and robot j at time k.
Still further, the method further comprises the steps of,the method comprises the following steps:
wherein k is μ To control estimated valueGain of convergence speed; />An estimated value of the difference of the distance measured values between the robot i and the robot j at the moment k-1; bi][j]Indicating whether robot i estimates the difference in distance measurement between robot i and robot j, 1 no, and b+b T =0, B is each B [ i ]][j]A matrix of components.
Still further, the target speed is:
wherein [ the f v x (k), f v y (k), f ω(k)]For the target speed of the multi-robot formation under the robot formation coordinate system at the time k, f v x (k)、 f v y (k) The velocity components of the target line velocity in the x and y directions respectively, f ω (k) is the target angular velocity; [ v dx (k),v dy (k),ω d (k)]Multi-robot braiding for k timeTarget speed, v, in global coordinate system dx (k)、v dy (k) The velocity components of the target line velocity in the x and y directions, ω (k) being the target angular velocity; [ x ] e (k),y e (k),e θ (k)]For the deviation of the current pose of the multi-robot formation at the moment k relative to the target pose, x e (k)、y e (k) The components of the distance deviation in the x and y directions, e θ (k) Is the angular deviation; k (k) p To control gain in proportion, k i In order to integrate the control gain,a transformation matrix of the global coordinate system to the robot formation coordinate system.
Still further, the second speed is:
wherein, i v″ x,i (k)、 i v″ y,i (k) The velocity components of the second velocity of the robot i in the x and y directions under the body coordinate system are respectively the k moment; [ f v x (k), f v y (k), f ω(k)]For the target speed of the multi-robot formation under the robot formation coordinate system at the time k, f v x (k)、 f v y (k) The velocity components of the target line velocity in the x and y directions respectively, f ω (k) is the target angular velocity; d, d (i,j) (k) The measurement distance between the robot i and the robot j at the moment k;a motion parameter component corresponding to the positive x-direction linear velocity, the positive y-direction linear velocity and the counter-clockwise angular velocity of the robot formation coordinate system, i x j (k)、 i y j (k) The coordinates of the robot j in the x and y directions under the coordinate system of the robot i at the moment k are respectively, N i Is a collection of robots adjacent to robot i.
Still further, each robot in S1 measures the distance between the robot and the neighboring robot at the current time, respectively, including: the robot predicts the expected coordinates of each adjacent robot at the current moment according to the displacement from the last moment to the current moment and the coordinates of each adjacent robot at the last moment under the coordinate system; determining candidate coordinates of each adjacent robot based on the laser radar point cloud data at the current moment, calculating the distance between each candidate coordinate and the expected coordinate closest to the candidate coordinate, and eliminating the corresponding candidate coordinate if the distance is greater than a distance threshold; pairing the removed candidate coordinates with the expected coordinates, wherein the candidate coordinates successfully paired and the expected coordinates unsuccessfully paired are final coordinates of all adjacent robots at the current moment, and the distance between the robot and each adjacent robot is calculated according to the final coordinates.
Still further, between S1 and S2, further includes: the piloting robot measures the distance between the piloting robot and each adjacent robot, measures the distance between each robot in the sight of the piloting robot and the neighbor of the robot, calculates the deviation between each distance and the corresponding expected distance, and judges that the multi-robot formation shape is consistent with the expected formation shape when the deviation is smaller than a deviation threshold value, otherwise, judges that the multi-robot formation shape is inconsistent with the expected formation shape.
According to another aspect of the present invention, there is provided a laser radar-based multi-robot formation tracking sampling control system, comprising: the system comprises a plurality of robots and a router, wherein the robots comprise a pilot robot and a following robot, the pilot robot sends information to the following robot through a network generated in the router, and the robots execute the multi-robot formation tracking sampling control method based on the laser radar.
In general, through the above technical solutions conceived by the present invention, the following beneficial effects can be obtained: based on sampling communication, measurement and discrete control models in each period, discrete control instructions are obtained by calculating the states of adjacent robots acquired by sampling, so that the method has practicability; by designing a distance measurement value deviation estimator, the circular motion beyond the expectations caused by inconsistent measurement values of the distances between adjacent robots is eliminated; the piloting robot calculates the due tracking speed of formation according to the observed formation pose, the following robot converts the tracking speed into a local speed according to the observation of the adjacent robot, and the formation distortion phenomenon caused by the abrupt change of the target motion state is reduced; and the sampling control cycle time is designed, so that the stability of the multi-robot system under sampling measurement and communication is ensured.
Drawings
FIG. 1 is a flow chart of a multi-robot formation tracking sampling control method based on laser radar according to an embodiment of the present invention;
FIG. 2 is a flow chart of the method of FIG. 1 for measuring the position of adjacent robots using lidar;
fig. 3 is a schematic diagram of a motion model of an omnidirectional mobile robot according to an embodiment of the present invention;
FIG. 4 is a graph of inter-formation errors when multiple robots are forming a formation according to an embodiment of the present invention;
FIG. 5 is a diagram of a plurality of robots forming a team according to an embodiment of the present invention;
FIG. 6 is a diagram of a centroid trace of a plurality of robots when the plurality of robots track a target according to an embodiment of the present invention;
FIG. 7 is a graph of error between the centroid pose of a plurality of robots and the pose of a target when the plurality of robots track the target according to the embodiment of the present invention;
FIG. 8 is a graph of inter-robot distance errors when multiple robots track a target according to an embodiment of the present invention;
fig. 9 is a diagram of a track of multiple robots when the multiple robots track a target according to an embodiment of the present invention.
Detailed Description
The present invention will be described in further detail with reference to the drawings and examples, in order to make the objects, technical solutions and advantages of the present invention more apparent. It should be understood that the specific embodiments described herein are for purposes of illustration only and are not intended to limit the scope of the invention. In addition, the technical features of the embodiments of the present invention described below may be combined with each other as long as they do not collide with each other.
In the present invention, the terms "first," "second," and the like in the description and in the drawings, if any, are used for distinguishing between similar objects and not necessarily for describing a particular sequential or chronological order.
Fig. 1 is a flowchart of a multi-robot formation tracking sampling control method based on a laser radar according to an embodiment of the present invention. Referring to fig. 1, in conjunction with fig. 2 to 9, a method for controlling multi-robot formation tracking sampling based on laser radar in this embodiment will be described in detail, and the method includes operations S1 to S3.
In operation S1, each robot measures the distance between itself and the adjacent robot at the current time, and calculates the first speed of the robot in its own coordinate system based on the measured distance, with the goal that the formation shape of the multiple robots approaches the expected formation shape.
Robots in the multi-robot system are respectively a pilot robot with global positioning capability and a following robot without global positioning capability according to whether the robots have two different configurations of global positioning capability. The pilot robot may use a global positioning system (Global Positioning System, GPS), RTK, instant positioning and mapping (Simultaneous Localization and Mapping, SLAM), or visual positioning devices to perform global positioning functions. The router is used for generating a local area network, each robot is connected into the local area network, and in the local area network generated by the router, the pilot robot can send instructions to the following robots through the ROS network, and the following robots cannot send information to other robots. Each robot adopts a gyroscope and an encoder to measure the speed and the gesture, and adopts a laser radar as a distance sensor.
All robots are all omni-directional mobile robots, including a main controller and a drive controller. Each robot uses a Mecanum wheel as a wheel, for example, so as to achieve the aim of omnidirectional movement. The main controller is used for completing planning of movement and calculating the target speed of the robot. The driving controller is used for driving the direct current motor to reach the target speed under the control of the main controller.
In this embodiment, the dynamics model of each robot in the multi-robot system is constructed as follows:
wherein x is i (k)、y i (k) The x-coordinate and y-coordinate of the robot i at time k, i=1, 2, respectively, n, n being the total number of robots in the multi-robot system,the speeds of the robot i in the x direction and the y direction at the k moments are respectively, T is the time difference between two adjacent moments, and +.>And remains unchanged from time k to time k+1.
According to an embodiment of the invention, the time difference T between two adjacent moments is:
wherein sigma is the preset maximum displacement of the robot between two adjacent moments, v max Is the maximum movement speed, k of the robot c First control gain, lambda, for multi-robot formation max (L) is the maximum eigenvalue of L, L is the Laplacian matrix of the topology graph G describing the robot neighbor relation.
Each robot selects two robots in the laser radar sight as adjacent robots, and the coordinates of the adjacent robots under the body coordinate system are measured by using the laser radars. Each robot achieves a desired formation shape by controlling the distance between the robot and the adjacent robot, respectively, the desired formation being defined by an undirected graph g= (V, a, D), v= { 1.
According to an embodiment of the present invention, each robot in operation S1 separately measures a distance between the robot and an adjacent robot at a current time, including sub-operation S11-sub-operation S13.
In sub-operation S11, the robot predicts the expected coordinates of each neighboring robot at the current time based on its displacement from the previous time to the current time and the coordinates of each neighboring robot at the previous time in its coordinate system.
Specifically, referring to fig. 2, taking a robot i as an example, reading data of a gyroscope and an encoder of the robot i, and calculating displacement deltax and deltay from k-1 moment to k moment of the robot i; the coordinate system Sigma of the adjacent robot in the robot i according to the k-1 moment i The coordinates, displacement deltax, displacement deltay below calculate the position of the adjacent robot at time k when the adjacent robot remains stationary Σ i The coordinates to be provided are the expected coordinates of each adjacent robot at the moment k.
In sub-operation S12, candidate coordinates of each neighboring robot are determined based on the lidar point cloud data at the current time, and a distance between each candidate coordinate and the nearest expected coordinate is calculated, and if the distance is greater than the distance threshold, the corresponding candidate coordinate is removed.
Firstly, reading laser radar point cloud data at the moment k, dividing the point cloud data into a plurality of continuous subsections, wherein the division basis is that the distance between two continuous points is minimum. Specifically, the laser radar point cloud data at the moment k is read, the point cloud data is represented in a polar coordinate form, the resolution is 1 degree, 360 points are included, the distance between every two adjacent data points is calculated, if the distance is larger than delta, the two points are considered not to belong to the same subsection, otherwise, the two points belong to the same subsection, delta represents that if the two points belong to the same object, delta is predefined, all the subsections are calculated, and the corresponding center is calculated.
And secondly, comparing the lengths of the adjacent robots, eliminating the subsections with the length difference larger than the tolerance threshold value xi, and determining the central coordinates of the rest subsections as candidate coordinates of the adjacent robots.
And finally, calculating the distance between each candidate coordinate and the nearest expected coordinate, and if the distance is greater than a distance threshold sigma, eliminating the corresponding candidate coordinate.
In sub-operation S13, the candidate coordinates after the elimination are paired with the expected coordinates, the candidate coordinates successfully paired and the expected coordinates unsuccessfully paired are final coordinates of each adjacent robot at the current moment, and the distances between the robot and each adjacent robot are calculated according to the final coordinates.
Specifically, pairing of candidate coordinates and expected coordinates is established, for example, by using the hungarian algorithm, and the pairing target is that the sum of distances between all matched candidate coordinates and expected coordinates is the shortest. Due to laser radar errors and mutual interference, the condition that the adjacent robots are not detected may occur, at this time, the number of candidate coordinates is smaller than that of the expected coordinates, and part of the expected coordinates cannot be paired, so that the expected coordinates which are not paired successfully are taken as the real coordinates of the adjacent robots which are not detected, and the candidate coordinates which are paired successfully are taken as the real coordinates of the adjacent robots which are detected. And calculating the distance between the robot and each adjacent robot according to the finally obtained real coordinates.
According to an embodiment of the invention, each robot calculates a first speed for forming and maintaining a multi-robot formation according to a distance-based distributed multi-robot formation generation sampling controller in the form of:
wherein, i v′ x,i (k)、 i v′ y,i (k) The velocity components of the first velocity of the robot i in the x and y directions under the body coordinate system at the moment k, i x j (k)、 i y j (k) The coordinates of the robot j in the x and y directions under the coordinate system of the robot i at the moment k are respectively, and d (i,j) (k) For the measured distance between robot i and robot j at time k，N i K is the set of robots adjacent to robot i c First control gain, D, for multi-robot formation (i,j) For a desired distance between robot i and robot j,an estimated value of the difference between the distance measurement values of robot i and robot j at time k. d, d (i,j) (k) Measured by robot i,/->Estimated by robot i.
Estimated value of difference between distance measurement values of robot i and robot j at time kThe method comprises the following steps:
wherein k is μ To control estimated valueGain of convergence speed; />An estimated value of the difference of the distance measured values between the robot i and the robot j at the moment k-1; bi][j]Indicating whether or not robot i estimates the difference in distance measurement between robot i and robot j, 1 is not estimated, 0 indicates that there is no adjacent relationship between robot i and robot j, and b+b T =0, B is each B [ i ]][j]A matrix of components. The robot achieves the aim of achieving the expected formation shape by controlling the distance between the robot and the adjacent robot to achieve the expected distance, and in order to achieve the aim, the topological graph needs to be a minimum rigidity graph which can only be moved and rotated integrally, and the length change of any side can lead to the formationDistortion of the team.
And S2, when the shape of the multi-robot formation is consistent with the expected formation shape, the pilot robot in the multi-robot calculates the target speed of the multi-robot formation under the robot formation coordinate system by using the PI control method according to the current pose, the current speed and the target pose of the multi-robot formation, and sends the target speed to other robots in the multi-robot.
Before performing operation S2, according to an embodiment of the present invention, it should further include: the pilot robot judges whether the multi-robot formation shape reaches an expected formation shape. Specifically, the piloting robot measures the distance between the piloting robot and each adjacent robot, measures the distance between each robot in the sight of the piloting robot and the neighbor of the robot, calculates the deviation between each distance and the corresponding expected distance, and judges that the multi-robot formation shape is consistent with the expected formation shape when the deviation is smaller than the deviation threshold value, otherwise, judges that the multi-robot formation shape is inconsistent with the expected formation shape.
Further, when the multi-robot formation shape is consistent with the expected formation shape, the pilot robot calculates the coordinates of the formation coordinate system origin under the global coordinate system and the rotation angle of the formation coordinate system relative to the global coordinate system through the observer.
The coordinates [ x (k), y (k) ] of the origin of the formation coordinate system under the global coordinate system are:
the formation coordinate system is fixed on the robot formation, the origin of the formation coordinate system is the centroid of the formation, and the positive directions of the x axis and the y axis are manually designated by an operator. [ l x n1 (k), l y n1 (k)]、[ l x n2 (k), l y n2 (k)]Respectively the coordinates of two adjacent robots observed by the piloting robot at the moment k, alpha l As a first coefficient, beta l Is the second coefficient, [ x ] l (k),y l (k)]For piloting the global coordinates of the robot at time k,the rotation matrix for piloting the robot coordinate system to the global coordinate system can be expressed as:
the coordinates of the formation centroid under the piloting robot coordinate system are obtained by adding the coordinate vectors of two adjacent robots: l x c (k)＝[α l x n1 (k)+β l x n2 (k)]、 l y c (k)＝[α l y n1 (k)+β l y n2 (k)]. The rotation angle θ (k) of the formation coordinate system with respect to the global coordinate system is:
wherein θ l (k) The attitude of the piloting robot k moment under the global coordinate system; θ 0 When the multi-robot formation and the pilot robot have the same gesture, the multi-robot formation is in the gesture under the pilot robot coordinate system.
The pilot robot calculates the target speed of the formation according to the current pose, the current speed and the target pose of the multi-robot formation and a master-slave type formation tracking sampling controller, and sends the target speed to a follower, wherein the master-slave type formation tracking sampling controller is in the form of:
wherein [ the f v x (k), f v y (k), f ω(k)]For the target speed of the multi-robot formation under the robot formation coordinate system at the time k, f v x (k)、 f v y (k) The velocity components of the target line velocity in the x and y directions respectively, f ω (k) is the target angular velocity; [ v dx (k),v dy (k),ω d (k)]Target speed, v, in global coordinate system for multi-robot formation at time k dx (k)、v dy (k) The velocity components of the target line velocity in the x and y directions, ω (k) being the target angular velocity; [ x ] e (k),y e (k),e θ (k)]For the deviation of the current pose of the multi-robot formation at the moment k relative to the target pose, x e (k)、y e (k) The components of the distance deviation in the x and y directions, e θ (k) Is the angular deviation; k (k) p To control gain in proportion, k i In order to integrate the control gain,a transformation matrix of the global coordinate system to the robot formation coordinate system.
According to an embodiment of the present invention, when the multi-robot formation shape is inconsistent with the expected formation shape, between the operations S1 and S3 include: the pilot robot sets the target speed of the multi-robot formation in the robot formation coordinate system to 0 and sends the target speed to other robots.
And S3, each robot takes the formation speed of the multiple robots as a target to keep the target speed, calculates a second speed of the robot under the body coordinate system according to the measured distance, calculates the combined speed of the first speed and the second speed, and controls the speed of the robot from the current moment to the next moment to be equal to the combined speed.
Each robot calculates the actual speed of the robot under the body coordinate system according to the target speed of the multi-robot formation, and sends the actual speed to the driving controller, the driving controller executes the operation of driving the robot formation to track the target under the condition of keeping the target shape, and the combined speed of the robot under the body coordinate system is as follows:
wherein, i v″ x,i (k)、 i v″ y,i (k) The velocity components of the second velocity of the robot i in the x and y directions under the body coordinate system are respectively the k moment; [ f v x (k), f v y (k), f ω(k)]For the target speed of the multi-robot formation under the robot formation coordinate system at the time k, f v x (k)、 f v y (k) The velocity components of the target line velocity in the x and y directions respectively, f ω (k) is the target angular velocity; d, d (i,j) (k) The measurement distance between the robot i and the robot j at the moment k;a motion parameter component corresponding to the positive x-direction linear velocity, the positive y-direction linear velocity and the counter-clockwise angular velocity of the robot formation coordinate system, i x j (k)、 i y j (k) The coordinates of the robot j in the x and y directions under the coordinate system of the robot i at the moment k are respectively, N i Is a collection of robots adjacent to robot i. d, d (i,j) (k) Measured by robot i.
In this embodiment, the effect of tracking and sampling control of multiple robot formations based on the laser radar is illustrated by taking a hybrid architecture of STM 32-JetsonnNano as an example for each robot. The STM32 embedded control system is based on a microcontroller and a real-time operating system, realizes a real-time control layer for controlling the motion of a vehicle body, and is provided with a nine-axis gyroscope to acquire the gesture. And the JetsonNano provided with the ROS is used as an upper controller to carry out planning on the robot so as to realize the cooperative behavior of the multi-robot system. Omnidirectional movement of the robot is achieved by using a Mecanum wheel, and a robot motion model is shown in FIG. 3.
In order to sense the external environment, the robot is provided with RPlidar-A1, which is a low-cost laser radar, the detection distance can reach 12m, the accuracy is 0.2 percent at most, and the measurement frequency of 8000 points per second is realized. The robot will use it to obtain the position of the neighbor within its local coordinate system.
The software architecture of the robot is based on the ROS, because the ROS provides a large number of standard interfaces and packages for the development of the robot, the development of the robot is accelerated, meanwhile, the ROS also well supports the distributed computation of multiple robots, and a plurality of robots can be easily connected into a multi-robot system through the ROS.
Through the router, a plurality of robots running the ROS system can conveniently form a multi-robot system. By running one ROS MASTER node on the pilot, then connecting all robot nodes to the ROS MASTER URI. The multiple robot systems can conveniently communicate with each other.
For a multi-robot system with three robots, the topology can be expressed as:
the global position of the robot is acquired by a visual motion capturing system, and the initial placement coordinates of the robot are [ -0.8,0 respectively]、[0，-0.8]、[0，0.8]. The multi-robot formation tracking sampling control method based on the laser radar shown in the figure 1 is operated on the three robots, and is realized by C++ language, and the control period is 50ms and k p Take the value of 2, c takes the value of 0.2, k i The value is 0.1.
Without tracking the target, the results refer to fig. 4 and 5, and fig. 4 shows that the distance error between robots eventually oscillates around 0, the motion trajectory refers to fig. 5, and the plurality of robots eventually form a formation of the shape of the target.
In the case of tracking a target walking in a figure 8, the results are shown in figures 6, 7, 8 and 9. The track of the robot formation center is shown in fig. 6, the deviation of the robot formation and the target is shown in fig. 7, the formation error when the robot tracks the target is shown in fig. 8, and the track of each robot and the formation centroid is shown in fig. 9. As can be seen from fig. 6 to 9, in the steady state, the formation error and the tracking error do not exceed 1.5cm at maximum.
The embodiment of the invention also provides a laser radar-based multi-robot formation tracking sampling control system, which comprises a plurality of robots and routers. The plurality of robots includes a pilot robot, the number of which is preferably one, and a follower robot, the number of which may be two or more.
The pilot robot transmits information to the following robots through the network generated in the router, and the plurality of robots execute the multi-robot formation tracking sampling control method based on the laser radar shown in the above-mentioned fig. 1 to 9. For details not yet in this embodiment, please refer to the laser radar-based multi-robot formation tracking sampling control method in the embodiments shown in fig. 1-9, which is not described herein.
It will be readily appreciated by those skilled in the art that the foregoing description is merely a preferred embodiment of the invention and is not intended to limit the invention, but any modifications, equivalents, improvements or alternatives falling within the spirit and principles of the invention are intended to be included within the scope of the invention.
Claims (8)
1. A multi-robot formation tracking sampling control method based on laser radar is characterized by comprising the following steps:
s1, each robot respectively measures the distance between the robot and the adjacent robot at the current moment, takes the formation shape of the multiple robots as a target, and calculates the first speed of the robot under the body coordinate system according to the measured distance;
s2, when the shape of the multi-robot formation is consistent with the expected formation shape, the pilot robot in the multi-robot calculates the target speed of the multi-robot formation under a robot formation coordinate system according to the current pose, the target speed and the target pose of the multi-robot formation by using a PI control method, and sends the target speed to other robots in the multi-robot;
s3, each robot takes the formation speed of multiple robots as a target, calculates a second speed of the robot under the body coordinate system according to the measured distance, calculates a combined speed of the first speed and the second speed, and controls the speed of the robot from the current moment to the next moment to be equal to the combined speed;
the first speed is:
wherein, i v′ x,i (k)、 i v′ y,i (k) The velocity components of the first velocity of the robot i in the x and y directions under the body coordinate system at the moment k, i x j (k)、 i y j (k) The coordinates of the robot j in the x and y directions under the coordinate system of the robot i at the moment k are respectively, and d (i,j) (k) For the measured distance between robot i and robot j at time k, N i K is the set of robots adjacent to robot i c First control gain, D, for multi-robot formation (i,j) For a desired distance between robot i and robot j,an estimated value of the difference of the distance measured values between the robot i and the robot j at the moment k;
the second speed is:
wherein iv' x,i (k)、iv” y,i (k) The velocity components of the second velocity of the robot i in the x and y directions under the body coordinate system are respectively the k moment; [ f v x (k), f v y (k), f ω(k)]For the target speed of the multi-robot formation under the robot formation coordinate system at the time k, f v x (k)、 f v y (k) The velocity components of the target line velocity in the x and y directions respectively, f ω (k) is the target angular velocity;forming motion parameter components corresponding to the positive x-direction linear velocity, the positive y-direction linear velocity and the counter-clockwise angular velocity of the coordinate system for the robot;
the combination speed is as follows:
2. the multi-robot formation tracking sampling control method based on laser radar according to claim 1, wherein when the multi-robot formation shape is not consistent with the expected formation shape, the steps between S1 and S3 include: the pilot robot sets the target speed of the multi-robot formation in the robot formation coordinate system to 0 and sends the target speed to other robots.
3. The laser radar-based multi-robot formation tracking sampling control method according to claim 1, wherein the time difference T between adjacent two moments is:
wherein sigma is the preset maximum displacement of the robot between two adjacent moments, v max Is the maximum movement speed, k of the robot c First control gain, lambda, for multi-robot formation max (L) is the maximum eigenvalue of L, L is the Laplacian matrix of the topology graph G describing the robot neighbor relation.
4. The multi-robot formation tracking sampling control method based on the laser radar according to claim 1, wherein,the method comprises the following steps:
wherein k is μ To control estimated valueGain of convergence speed; />An estimated value of the difference of the distance measured values between the robot i and the robot j at the moment k-1; bi][j]Indicating whether robot i estimates the difference in distance measurement between robot i and robot j, 1 no, and b+b T =0, B is each B [ i ]][j]A matrix of components.
5. The laser radar-based multi-robot formation tracking sampling control method according to claim 1, wherein the target speed is:
wherein [ the f v x (k), f v y (k), f ω(k)]For the target speed of the multi-robot formation under the robot formation coordinate system at the time k, f v x (k)、 f v y (k) The velocity components of the target line velocity in the x and y directions respectively, f ω (k) is the target angular velocity; [ v dx (k),v dy (k),ω d (k)]For k moment multiple robotsForming a target speed, v, in a global coordinate system dx (k)、v dy (k) The velocity components of the target line velocity in the x and y directions, ω (k) being the target angular velocity; [ x ] e (k),y e (k),e θ (k)]For the deviation of the current pose of the multi-robot formation at the moment k relative to the target pose, x e (k)、y e (k) The components of the distance deviation in the x and y directions, e θ (k) Is the angular deviation; k (k) p To control gain in proportion, k i In order to integrate the control gain,a transformation matrix of the global coordinate system to the robot formation coordinate system.
6. The method for controlling multi-robot formation tracking sampling based on laser radar according to any one of claims 1 to 5, wherein each robot in S1 measures a distance from an adjacent robot at a current time, respectively, comprising:
the robot predicts the expected coordinates of each adjacent robot at the current moment according to the displacement from the last moment to the current moment and the coordinates of each adjacent robot at the last moment under the coordinate system;
determining candidate coordinates of each adjacent robot based on the laser radar point cloud data at the current moment, calculating the distance between each candidate coordinate and the expected coordinate closest to the candidate coordinate, and eliminating the corresponding candidate coordinate if the distance is greater than a distance threshold;
pairing the removed candidate coordinates with the expected coordinates, wherein the candidate coordinates successfully paired and the expected coordinates unsuccessfully paired are final coordinates of all adjacent robots at the current moment, and the distance between the robot and each adjacent robot is calculated according to the final coordinates.
7. The multi-robot formation tracking sampling control method based on laser radar according to any one of claims 1 to 5, wherein between S1 and S2 further comprises:
the piloting robot measures the distance between the piloting robot and each adjacent robot, measures the distance between each robot in the sight of the piloting robot and the neighbor of the robot, calculates the deviation between each distance and the corresponding expected distance, and judges that the multi-robot formation shape is consistent with the expected formation shape when the deviation is smaller than a deviation threshold value, otherwise, judges that the multi-robot formation shape is inconsistent with the expected formation shape.
8. A laser radar-based multi-robot formation tracking sampling control system, comprising: a plurality of robots and routers, the plurality of robots including a pilot robot and a following robot, the pilot robot transmitting information to the following robot through a network generated within the router, the plurality of robots performing the laser radar-based multi-robot formation tracking sampling control method according to any one of claims 1 to 7.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111626053.0A CN114326731B (en) | 2021-12-28 | 2021-12-28 | Multi-robot formation tracking sampling control method and system based on laser radar |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111626053.0A CN114326731B (en) | 2021-12-28 | 2021-12-28 | Multi-robot formation tracking sampling control method and system based on laser radar |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN114326731A CN114326731A (en) | 2022-04-12 |
| CN114326731B true CN114326731B (en) | 2024-04-09 |
Family
ID=81014979
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202111626053.0A Active CN114326731B (en) | 2021-12-28 | 2021-12-28 | Multi-robot formation tracking sampling control method and system based on laser radar |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114326731B (en) |
Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105527960A (en) * | 2015-12-18 | 2016-04-27 | 燕山大学 | Mobile robot formation control method based on leader-follow |
| CN108897321A (en) * | 2018-07-16 | 2018-11-27 | 重庆理工大学 | Based on navigating, the robot formation for following method can be changed formation control method and controller |
| CN111413964A (en) * | 2020-03-09 | 2020-07-14 | 上海理工大学 | Method for detecting moving state of obstacle in real time by mobile robot in environment |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US9146561B2 (en) * | 2013-12-03 | 2015-09-29 | King Fahd University Of Petroleum And Minerals | Robotic leader-follower navigation and fleet management control method |
-
2021
- 2021-12-28 CN CN202111626053.0A patent/CN114326731B/en active Active
Patent Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105527960A (en) * | 2015-12-18 | 2016-04-27 | 燕山大学 | Mobile robot formation control method based on leader-follow |
| CN108897321A (en) * | 2018-07-16 | 2018-11-27 | 重庆理工大学 | Based on navigating, the robot formation for following method can be changed formation control method and controller |
| CN111413964A (en) * | 2020-03-09 | 2020-07-14 | 上海理工大学 | Method for detecting moving state of obstacle in real time by mobile robot in environment |
Non-Patent Citations (2)
| Title |
|---|
| 基于领航者的多机器人系统编队控制研究;孙玉娇;杨洪勇;于美妍;;鲁东大学学报(自然科学版);20200128(第01期);全文 * |
| 孙玉娇 ; 杨洪勇 ; 于美妍 ; .基于领航者的多机器人系统编队控制研究.鲁东大学学报(自然科学版).2020,(第01期),全文. * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN114326731A (en) | 2022-04-12 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN107239076B (en) | AGV laser SLAM method based on virtual scanning and distance measurement matching | |
| CN107562048B (en) | Dynamic obstacle avoidance control method based on laser radar | |
| CN112882053B (en) | Method for actively calibrating external parameters of laser radar and encoder | |
| US20170010100A1 (en) | Map production method, mobile robot, and map production system | |
| CN107315171B (en) | Radar networking target state and system error joint estimation algorithm | |
| CN110166571A (en) | A kind of automatic follower method and device based on mobile robot | |
| JP5429901B2 (en) | Robot and information processing apparatus program | |
| CN109655059B (en) | Vision-inertia fusion navigation system and method based on theta-increment learning | |
| CN113701742B (en) | Mobile robot SLAM method based on cloud and edge fusion calculation | |
| CN113238554A (en) | Indoor navigation method and system based on SLAM technology integrating laser and vision | |
| Gao et al. | Localization of mobile robot based on multi-sensor fusion | |
| CN113160280B (en) | Dynamic multi-target tracking method based on laser radar | |
| CN114326731B (en) | Multi-robot formation tracking sampling control method and system based on laser radar | |
| CN116225029B (en) | Robot path planning method | |
| CN114840003B (en) | Single-pilot multi-AUV co-positioning and track tracking control method | |
| CN115993089B (en) | PL-ICP-based online four-steering-wheel AGV internal and external parameter calibration method | |
| Son et al. | Formation coordination for the propagation of a group of mobile agents via self-mobile localization | |
| Wang et al. | Visual regulation of a nonholonomic wheeled mobile robot with two points using Lyapunov functions | |
| Zali et al. | Localization of an indoor mobile robot using decentralized data fusion | |
| Wang et al. | Agv navigation based on apriltags2 auxiliary positioning | |
| Yan et al. | A method for position estimation of mobile robot based on data fusion | |
| Li et al. | Research on SLAM based on RBPF algorithm in indoor environment | |
| Wang et al. | Cooperative localization method for multi-robot based on PF-EKF | |
| CN117315175B (en) | Composition positioning device and method based on robot | |
| CN114637279B (en) | Multi-agent formation control method based on local azimuth information |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |