CN114740863A - Multi-machine formation control method and device based on piloting following - Google Patents
Multi-machine formation control method and device based on piloting following Download PDFInfo
- Publication number
- CN114740863A CN114740863A CN202210444690.4A CN202210444690A CN114740863A CN 114740863 A CN114740863 A CN 114740863A CN 202210444690 A CN202210444690 A CN 202210444690A CN 114740863 A CN114740863 A CN 114740863A
- Authority
- CN
- China
- Prior art keywords
- robot
- following
- current
- deviation
- coordinate
- 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.)
- Pending
Links
- 230000015572 biosynthetic process Effects 0.000 title claims abstract description 40
- 238000000034 method Methods 0.000 title claims abstract description 39
- 238000004364 calculation method Methods 0.000 claims description 18
- 230000005611 electricity Effects 0.000 claims description 2
- 238000005755 formation reaction Methods 0.000 description 28
- 230000033001 locomotion Effects 0.000 description 17
- 230000006870 function Effects 0.000 description 4
- 230000009286 beneficial effect Effects 0.000 description 3
- 238000013178 mathematical model Methods 0.000 description 3
- 238000006467 substitution reaction Methods 0.000 description 2
- 238000013461 design Methods 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 230000002349 favourable effect Effects 0.000 description 1
- 238000012423 maintenance Methods 0.000 description 1
- 230000014759 maintenance of location Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
- G05D1/0238—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using obstacle or wall sensors
- G05D1/024—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using obstacle or wall sensors in combination with a laser
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0287—Control of position or course in two dimensions specially adapted to land vehicles involving a plurality of land vehicles, e.g. fleet or convoy travelling
- G05D1/0291—Fleet control
- G05D1/0293—Convoy travelling
Landscapes
- Physics & Mathematics (AREA)
- Engineering & Computer Science (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Optics & Photonics (AREA)
- Electromagnetism (AREA)
- Manipulator (AREA)
Abstract
The invention discloses a multi-machine formation control method and a multi-machine formation control device based on navigation following, wherein the method comprises the following steps: presetting an initial relative coordinate between a piloting robot and a following robot; acquiring current coordinates and current speed information of a piloting robot; calculating expected coordinates, an expected orientation angle and expected speed information of the following robot based on the initial relative coordinates, the current coordinates of the piloting robot and the current speed information; acquiring a current coordinate and a current orientation angle of the following robot; calculating a coordinate deviation of the following robot based on the current coordinate and the expected coordinate of the following robot; calculating an orientation deviation of the following robot based on the current orientation angle and the desired orientation angle of the following robot; calculating output speed information of the following robot based on the coordinate deviation, the orientation deviation and the expected speed information of the following robot; and controlling the following robot to move based on the output speed information of the following robot. The method has the advantages of strong retentivity and synchronization.
Description
Technical Field
The invention relates to the field of robots, in particular to a multi-machine formation control method and device based on navigation following.
Background
In recent years, since robots can perform tasks that are difficult to be performed by a single robot by coordinating with each other, the robots are widely used in various fields such as performances, search and rescue, transportation, and the like. One of the methods for realizing robot formation is a piloting following method, that is, a tracking robot moves along with a piloting robot and keeps a preset relative distance with the piloting robot. The existing piloting following method generally comprises the steps of calculating an expected state of a following robot through the state of the piloting robot, and controlling the following robot according to an error between an actual state and the expected state of the following robot.
In the control algorithm of the existing pilot following method, the expected orientation of the following robot and the current orientation of the pilot robot are consistent by default, but the orientation of the following robot is not favorable for the following robot to keep formation when the pilot robot performs turning motion. In addition, the control algorithm of the existing pilot following method has the problem that the following robot can start to move only after an error is generated, so that the following robot has certain hysteresis when moving just beginning.
Disclosure of Invention
The invention aims to provide a multi-machine formation control method and device based on navigation following to solve the related problems at least to a certain extent.
In order to achieve the purpose, the invention adopts the following technical scheme:
a multi-machine formation control method based on piloting following comprises the following steps:
presetting an initial relative coordinate between a piloting robot and a following robot;
acquiring current coordinates and current speed information of the piloting robot;
calculating desired coordinates, a desired orientation angle, and desired speed information of the following robot based on the initial relative coordinates, the current coordinates of the piloting robot, and the current speed information;
acquiring the current coordinate and the current orientation angle of the following robot;
calculating a coordinate deviation of the following robot based on the current coordinate and the desired coordinate of the following robot;
calculating an orientation deviation of the following robot based on a current orientation angle and a desired orientation angle of the following robot;
calculating output speed information of the following robot based on the coordinate deviation, the orientation deviation and the expected speed information of the following robot;
and controlling the following robot to move based on the output speed information of the following robot.
Optionally, the current speed information of the piloting robot includes a current angular speed and a current linear speed, the expected speed information of the following robot includes an expected angular speed and an expected linear speed, and the output speed information of the following robot includes an output angular speed and an output linear speed;
the coordinate deviation of the following robot comprises left and right deviation and front and back deviation;
the steps are as follows: calculating expected coordinates, an expected orientation angle and expected speed information of the following robot based on the initial relative coordinates, the current coordinates of the piloting robot and the current speed information, and specifically comprising the following steps of:
calculating an expected coordinate of the following robot based on the initial relative coordinate and a current coordinate of a piloting robot;
calculating the turning radius of the piloting robot based on the current angular velocity and the current linear velocity of the piloting robot;
calculating a desired heading angle of the following robot based on the initial relative coordinates and a turning radius of the piloting robot;
calculating an expected angular velocity of the following robot based on the current angular velocity of the piloted robot;
calculating an expected linear velocity of the following robot based on the initial relative coordinates, and a current angular velocity, a current linear velocity, and a turning radius of the piloting robot;
the steps are as follows: calculating output speed information of the following robot based on the coordinate deviation, the orientation deviation and the expected speed information of the following robot, and specifically comprising:
calculating an output linear velocity of the following robot based on the desired linear velocity and the front-to-back deviation of the following robot;
calculating an output angular velocity of the following robot based on the desired angular velocity, the left-right deviation, and the orientation deviation of the following robot.
Optionally, the desired coordinates of the following robot are expressed as (x)r，yr) Calculated according to the following formula:
xr＝x0+a；
yr＝y0+b；
wherein (a, b) are the initial relative coordinates, (x)0，y0) The current coordinate of the piloting robot is obtained;
the turning radius of the piloted robot is represented as r0Calculated according to the following formula:
wherein v is0Is the current linear velocity, w, of the piloting robot0The current angular velocity of the piloted robot;
the desired orientation angle of the following robot is denoted as θrCalculated according to the following formula:
the desired angular velocity of the following robot is denoted wrCalculated according to the following formula:
wr＝w0；
the desired linear velocity of the following robot is denoted vrCalculated according to the following formula:
wherein, the wtIs a preset angular velocity threshold and is a positive value;
the steps are as follows: acquiring the current coordinate and the current speed information of the piloting robot, wherein the method also comprises the following steps: a preset angular velocity threshold.
Optionally, the left-right deviation of the following robot is represented as x, the front-back deviation is represented as y, and the following formula is calculated:
x＝x1-xr；
y＝y1-yr；
wherein (x)1，y1) The current coordinates of the following robot are obtained;
the orientation deviation of the following robot is represented as theta and is calculated according to the following formula:
θ＝θ1-θr；
wherein, theta1Is the current orientation angle of the following robot.
The output linear velocity of the following robot is denoted by veCalculated according to the following formula:
ve＝vr+k1y；
wherein k is1Is a predetermined constant and is a negative value;
the output angular velocity of the following robot is denoted weCalculated according to the following formula:
we＝wr+f(k2)x+k3θ；
wherein k is2And k3Are all preset constants and positive values.
Optionally, the steps of: controlling the following robot to move based on the output speed information of the following robot, and then further comprising:
and returning to the step: and acquiring the current coordinate and the current speed information of the piloting robot.
Optionally, a navigation laser radar and a navigation encoder are installed on the navigation robot, the current coordinate of the navigation robot is output after being collected by the navigation laser radar, and the current speed information of the navigation robot is output after being collected by the navigation encoder;
and the following laser radar is installed on the following robot, and the current coordinate and the current orientation angle of the following robot are both acquired by the following laser radar and then output.
A multi-machine formation control device based on piloting following comprises:
the setting module is used for presetting an initial relative coordinate between the piloting robot and the following robot;
the navigation acquisition module is used for acquiring the current coordinate and the current speed information of the navigation robot;
an expected calculation module for calculating expected coordinates, an expected orientation angle and expected speed information of the following robot based on the initial relative coordinates, the current coordinates of the piloting robot and the current speed information;
the following acquisition module is used for acquiring the current coordinate and the current orientation angle of the following robot;
a coordinate deviation calculation module for calculating a coordinate deviation of the following robot based on a current coordinate and an expected coordinate of the following robot;
an orientation deviation calculation module for calculating an orientation deviation of the following robot based on a current orientation angle and a desired orientation angle of the following robot;
the output calculation module is used for calculating the output speed information of the following robot based on the coordinate deviation, the orientation deviation and the expected speed information of the following robot;
and the control module is used for controlling the following robot to move based on the output speed information of the following robot.
Optionally, the current speed information of the piloting robot includes a current angular speed and a current linear speed, the expected speed information of the following robot includes an expected angular speed and an expected linear speed, and the output speed information of the following robot includes an output angular speed and an output linear speed;
the coordinate deviation of the following robot comprises left and right deviation and front and back deviation;
the expectation calculation module includes:
an expected coordinate unit for calculating an expected coordinate of the following robot based on the initial relative coordinate and a current coordinate of a piloting robot;
a turning radius unit for calculating a turning radius of the piloted robot based on the current angular velocity and the current linear velocity of the piloted robot;
a desired orientation angle unit for calculating a desired orientation angle of the following robot based on the initial relative coordinates and a turning radius of the piloting robot;
an expected angular velocity unit for calculating an expected angular velocity of the following robot based on a current angular velocity of the piloting robot;
an expected linear velocity unit for calculating an expected linear velocity of the following robot based on the initial relative coordinates, and the current angular velocity, the current linear velocity, and the turning radius of the piloting robot;
the output calculation module includes:
an output linear velocity unit for calculating an output linear velocity of the following robot based on a desired linear velocity and a front-to-back deviation of the following robot;
an output angular velocity unit for calculating an output angular velocity of the following robot based on a desired angular velocity, a left-right deviation, and an orientation deviation of the following robot;
the control module is also used for triggering the pilot acquisition module;
the navigation robot is provided with a navigation laser radar and a navigation encoder, the navigation laser radar is used for outputting the current coordinate of the navigation robot after being collected, the navigation encoder is used for outputting the current speed information of the navigation robot after being collected, and the navigation acquisition module is electrically connected with the navigation laser radar and the navigation encoder;
follow the laser radar of installing on the robot, follow the laser radar and be used for gathering the back output follow the current coordinate and the current orientation angle of robot, follow and acquire the module electricity and connect follow the laser radar.
Optionally, the desired coordinates of the following robot are expressed as (x)r，yr) Calculated according to the following formula:
xr＝x0+a；
yr＝y0+b；
wherein (a, b) are the initial relative coordinates, (x)0，y0) Is the current coordinate of the piloted robot;
the turning radius of the piloting robot is represented as r0Calculated according to the following formula:
wherein v is0Is the current linear velocity, w, of the piloted robot0Is the current angular velocity of the piloted robot;
the desired orientation angle of the following robot is denoted as θrCalculated according to the following formula:
the desired angular velocity of the following robot is denoted wrCalculated according to the following formula:
wr＝w0；
the desired linear velocity of the following robot is denoted vrCalculated according to the following formula:
wherein, the wtIs a preset angular velocity threshold and is a positive value;
the setting module is also used for presetting an angular speed threshold;
the left deviation and the right deviation of the following robot are expressed as x, the front deviation and the rear deviation are expressed as y, and the following formula is calculated:
x＝x1-xr；
y＝y1-yr；
wherein (x)1，y1) Is the current coordinates of the following robot;
the orientation deviation of the following robot is represented as theta and is calculated according to the following formula:
θ＝θ1-θr；
wherein, theta1Is the current orientation angle of the following robot.
The output linear velocity of the following robot is denoted veCalculated according to the following formula:
ve＝vr+k1y；
wherein k is1Is a predetermined constant and is a negative value;
the output angular velocity of the following robot is denoted weCalculated according to the following formula:
we＝wr+f(k2)x+k3θ；
wherein k is2And k3Are all preset constants and positive values.
A computer readable storage medium having stored thereon computer instructions which, when run on a processor of an electronic device, cause the electronic device to perform the method as previously described.
Compared with the prior art, the invention has the following beneficial effects:
the multi-machine formation control method provided by the invention considers the expected orientation angle and the orientation deviation of the following robot, wherein the expected orientation angle of the following robot is calculated based on the initial relative coordinate and the current speed information of the piloting robot, so that the expected orientation of the following robot can be adjusted according to the motion state of the piloting robot, the expected orientation of the following robot is more suitable for the motion state which the following robot should have when keeping the formation, the stability of the whole formation is better facilitated, and the formation is not easy to disorder. In addition, the output speed information of the following robot is calculated based on the coordinate deviation, the orientation deviation and the expected speed information of the following robot, so that the following robot can move along with the movement of the pilot robot no matter whether the deviation exists or not, if the deviation exists, the position of the following robot is continuously adjusted according to the deviation in the movement process to correct the deviation, the synchronism of the following robot and the pilot robot is effectively improved, and the formation retention is strong.
Drawings
In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings used in the description of the embodiments or the prior art will be briefly described below, and it is obvious that the drawings in the following description are only some embodiments of the present invention, and for those skilled in the art, other drawings can be obtained according to these drawings without inventive exercise.
Fig. 1 is a formation schematic diagram of a multi-machine formation control method based on pilot following according to an embodiment of the present invention.
Detailed Description
In order to make the objects, features and advantages of the present invention more obvious and understandable, the technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings in the embodiments of the present invention, and it is obvious that the embodiments described below are only a part of the embodiments of the present invention, and not all of the embodiments. All other embodiments, which can be derived by a person skilled in the art from the embodiments given herein without making any creative effort, shall fall within the protection scope of the present invention.
The technical scheme of the invention is further explained by the specific implementation mode in combination with the attached drawings.
The invention provides a multi-machine formation control method based on navigation following, referring to fig. 1, the multi-machine formation control method comprises steps S1-S8, which are as follows:
and S1, presetting initial relative coordinates between the pilot robot and the following robot.
The initial relative coordinates are expressed as (a, b), and as shown in fig. 1, the lateral distance between the center point when the piloting robot is in its current pose and the center point when the following robot is in its desired pose is a, and the longitudinal distance is b.
And S2, acquiring the current coordinate and current speed information of the piloting robot.
As shown in fig. 1, the piloting robot moves according to the preset travel track, and the current coordinate of the piloting robot is the coordinate of the central point when the piloting robot is in the current pose, and is expressed as (x)0，y0)。
The current speed information of the piloting robot is the speed information when the piloting robot is in the current pose, and specifically comprises the current angular speed and the current linear speed. The current angular velocity of the piloted robot is denoted w0The current linear velocity is denoted by v0。
Specifically, a navigation laser radar and a navigation encoder are installed on the navigation robot, the current coordinate of the navigation robot is output after being collected by the navigation laser radar, and the current speed information of the navigation robot is output after being collected by the navigation encoder.
And S3, calculating expected coordinates, an expected orientation angle and expected speed information of the following robot based on the initial relative coordinates, the current coordinates of the pilot robot and the current speed information.
Wherein the desired velocity information includes a desired angular velocity and a desired linear velocity.
Step S3 specifically includes S31-S35.
And S31, calculating expected coordinates of the following robot based on the initial relative coordinates and the current coordinates of the pilot robot.
As shown in fig. 1, the expected coordinates of the following robot are the coordinates of the center point when the following robot is in the expected pose, specifically expressed as (x)r，yr) Calculated according to the following formula:
xr＝x0+a；
yr＝y0+b；
as described hereinbefore, wherein (x)0，y0) As the current coordinates of the piloting robot, (a, b) as the initial relative coordinates between the piloting robot and the following robot.
And S32, calculating the turning radius of the pilot robot based on the current angular velocity and the current linear velocity of the pilot robot.
As shown in fig. 1, the turning radius of the piloted robot is the radius of the preset driving track of the piloted robot, specifically denoted as r0Calculated according to the following formula:
as described hereinbefore, wherein v0Is the current linear velocity, w, of the piloting robot0Is the current angular velocity of the piloted robot.
And S33, calculating the expected orientation angle of the following robot based on the initial relative coordinates and the turning radius of the pilot robot.
As shown in fig. 1, the orientation of the following robot (the line with an arrow in fig. 1) is the direction of the head of the following robot, and the orientation angle of the following robot is the angle formed between the orientation of the following robot and the Y-direction line (the dotted line in fig. 1) passing through the center point of the following robot. When the orientation of the following robot is positioned on the right side of the Y-direction straight line, the orientation angle is set to be a positive value; when the orientation of the following robot is located on the left side of the Y-direction straight line, its orientation angle is determined to be a negative value.
The desired orientation of the following robot is the orientation of the following robot when in the desired pose, and is tangent to the desired travel trajectory of the following robot. FollowingThe desired orientation angle of the robot is denoted θrFrom the geometric relationship, as can be seen from FIG. 1, θrShould be equal to the sum theta2Angle of (a), i.e. thetarCan be calculated according to the following formula:
as previously described, where (a, b) is the initial relative coordinate between the lead robot and the following robot, r0Is the turning radius of the piloted robot.
In the mathematical model adopted by the invention, the expected orientation angle of the following robot is calculated based on the initial relative coordinate and the turning radius (obtained by calculating the current speed information of the piloted robot) of the piloted robot, so that the expected orientation of the following robot can be adjusted according to the motion state of the piloted robot, and the expected orientation of the following robot is more suitable for the motion state due to the maintenance of the formation of the following robot, and the stability of the whole formation is more facilitated.
And S34, calculating the expected angular speed of the following robot based on the current angular speed of the pilot robot.
The expected angular speed of the following robot is the angular speed when the following robot is in an expected pose, and the current angular speed w of the pilot robot is required to be equal to0Always in agreement, the desired angular velocity of the following robot is then denoted wrCalculated according to the following formula:
wr＝w0。
and S35, calculating the expected linear speed of the following robot based on the initial relative coordinates and the current angular speed, the current linear speed and the turning radius of the pilot robot.
As shown in fig. 1, the desired linear velocity of the following robot is the linear velocity at which it is in the desired pose. When the absolute value of the current angular velocity of the piloting robot (the angular velocity is divided into positive and negative values) is smaller than a preset angular velocity threshold (the angular velocity threshold is a positive constant, such as 0.05rad/s), the piloting robot is judged to be in a linear motion state, and at the moment, the expected linear velocity of the following robot is consistent with the current linear velocity of the piloting robot.
And when the absolute value of the current angular velocity of the piloting robot is greater than or equal to a preset angular velocity threshold value, judging that the piloting robot is in a turning motion state (the turning motion state comprises autorotation). The desired linear velocity of the following robot at this time is calculated as follows:
the desired linear velocity of the following robot is denoted vrThe radius following the desired travel path of the robot is denoted as rr；
From the calculation formula of v ═ wr, v can be obtainedr＝vrwr；
From FIG. 1, r can be seenr＝r0+rdSubstituting v intor＝vrwrV. availabler＝wr(r0+rd)＝wrr0+wrrd；
According to the foregoing it is known that wr＝w0Substituting v intor＝wrr0+wrrdV. availabler＝w0r0+w0rd＝v0+w0rd；
From the geometrical relationships of FIG. 1, it can be seen
Substitution vr＝v0+w0rdIs obtained by
In summary, vrCalculated according to the following formula:
wherein, the wtIs a preset angular velocity threshold and is positive. Step S2 is preceded by the step of: a preset angular velocity threshold.
Can understand thatIs that w0＜wtThe limiting condition of (2) may be changed to w0≤wtIn this state w0≥wtIs changed to w0＞wt. Simple alternatives to such limitations are also intended to be within the scope of the present invention.
And S4, acquiring the current coordinates and the current orientation angle of the following robot.
As shown in fig. 1, the current coordinate of the following robot is the coordinate of the center point when the following robot is in the current pose, and may be specifically represented as (x)1，y1) The current orientation angle of the following robot is θ in fig. 11. And the following laser radar is arranged on the following robot, and the current coordinate and the current orientation angle of the following robot are acquired by the following laser radar and then output.
And S5, calculating the coordinate deviation of the following robot based on the current coordinate and the expected coordinate of the following robot.
Specifically, the coordinate deviation includes a left-right deviation x and a front-back deviation y, and x and y are calculated according to the following formula:
x＝x1-xr；
y＝y1-yr；
as described hereinbefore, wherein (x)1，y1) To follow the current coordinates of the robot, (x)r，yr) To follow the desired coordinates of the robot.
And S6, calculating the orientation deviation of the following robot based on the current orientation angle and the expected orientation angle of the following robot.
The orientation deviation of the following robot is represented as θ, and is calculated according to the following formula:
θ＝θ1-θr；
as previously described, of which theta1To follow the current heading angle of the robot, thetarTo follow the desired heading angle of the robot.
And S7, calculating the output speed information of the following robot based on the coordinate deviation, the orientation deviation and the expected speed information of the following robot.
Specifically, the output speed information includes an output linear speed and an output angular speed.
Step S7 includes steps S71-S72.
And S71, calculating the output linear speed of the following robot based on the expected linear speed and the front-back deviation of the following robot.
In particular, the output linear velocity of the following robot is denoted veCalculated according to the following formula:
ve＝vr+k1y；
wherein k is1Is a predetermined constant and is negative.
While following the current ordinate y of the robot1<Following the desired ordinate y of the robotrWhen the front and rear deviation y is equal to y1-yr<0，k1y>0, increasing the speed of an output line of the following robot at the moment, and accelerating the following robot to correct the front and back deviation; while following the current coordinate y of the robot1>Following the desired coordinate y of the robotrWhen the front and rear deviation y is equal to y1-yr>0，k1y<0, the speed of an output line of the following robot is reduced, and the following robot is decelerated to correct the front deviation and the rear deviation; while following the current coordinate y of the robot1Desired coordinate y of the following robotrWhen the front and rear deviation y is equal to y1-yrAnd (5) keeping the linear speed of the following robot to be 0, and keeping the original speed of the following robot.
And S72, calculating the output angular speed of the following robot based on the expected angular speed, the left-right deviation and the orientation deviation of the following robot.
Specifically, the output angular velocity of the following robot is denoted as weCalculated according to the following formula:
we＝wr+f(k2)x+k3θ；
wherein k is2And k3Are all preset constants and positive values.
In the present invention, the output speed information of the following robot is calculated based on the coordinate deviation, the orientation deviation, and the desired speed information thereof, and it is known from the mathematical model that the output angular speed and the output linear speed of the following robot are not necessarily zero when the deviation (including the coordinate deviation and the orientation deviation) is zero. Therefore, the following robot can move along with the movement of the pilot robot no matter whether the deviation exists or not, and if the deviation exists, the position of the following robot is continuously adjusted according to the deviation in the movement process to correct the deviation. Therefore, the problem that the following robot can move only after deviation is generated in the traditional navigation following method is solved, the synchronism of the following robot and the navigation robot is effectively improved, and the formation retentivity is high.
In particular, when following the advance of the robot (if calculated veIf the following robot is more than 0, judging that the following robot is in a forward state, f (k)2)＝k2Is a positive constant. In this state, when the current pose of the following robot is to the left of the expected pose, the left-right deviation x ═ x1-xr<0，f(k2)x<0, reducing the angular speed of the following robot, and rotating the head to the right, namely the following robot advances and moves to the right to correct left and right errors; similarly, when the robot moves forward along with the robot and the current pose of the robot is right to the expected pose, the robot moves to the left while moving forward to correct left and right errors.
Following the robot back (if v is calculated)eIf not less than 0, judging that the following robot is in a retreating state, f (k)2)＝-k2Is a negative constant. In this state, when the current pose of the following robot is to the left of the expected pose, the left-right deviation x is x1-xr<0，f(k2) x is larger than 0, the angular speed of the following robot is increased, and the headstock rotates towards the left, namely, the following robot retreats and corrects the left and right errors by moving towards the right. Similarly, when the robot moves backwards along with the robot and the current pose of the robot is right side of the expected pose, the robot moves to the left side while moving backwards so as to correct left and right errors.
In conclusion, when the following robot moves forwards or backwards, the left-right error can be corrected by adjusting the direction of the head of the following robot.
k3The orientation deviation of the following robot can be corrected no matter whether the following robot is in a forward state or a backward state. For example, when the head of the following robot is biased to the left of the expected pose (as in the case of fig. 1), the heading of the following robot is biased toward the deviation theta<0，k3θ<0, reducing the angular speed of the following robot, and rotating the head to the right, namely rotating the following robot to the expected pose direction to correct the orientation deviation; similarly, when the head of the following robot is deviated to the right side of the expected pose, the head of the following robot rotates to the left (in the expected pose direction at this time) to correct the deviation.
And S8, controlling the following robot to move based on the output speed information of the following robot.
After completion of step S8, the process returns to step S2 to continue the next round of control.
In conclusion, the multi-machine formation control method has the following advantages:
1. the robot has wide application range, is suitable for the control of following robots of various formations, including but not limited to transverse or longitudinal formations; it is also suitable for maintaining formation of multiple robots under various motions, including but not limited to forward, backward, turning or autorotation states.
2. The method is simple and easy to implement, and the specifically adopted mathematical model is simple and has strong portability.
3. The expected orientation angle and the orientation deviation of the following robot are considered, the expected orientation angle of the following robot is calculated based on the initial relative coordinate and the current speed information of the piloting robot, the expected orientation of the following robot can be adjusted according to the motion state of the piloting robot, and therefore the expected orientation of the following robot is more suitable for the motion state of the following robot when the following robot keeps the formation, the stability of the whole formation is better facilitated, and the formation is not easy to disorder.
4. The synchronism is strong. The output speed information of the following robot is calculated based on the coordinate deviation, the orientation deviation and the expected speed information of the following robot, so that the following robot can move along with the motion of the piloting robot no matter whether the deviation exists or not, if the deviation exists, the position of the following robot is continuously adjusted according to the deviation in the motion process to correct the deviation, the synchronism of the following robot and the piloting robot is effectively improved, and the formation retentivity is high.
The invention also provides a multi-machine formation control device based on navigation following, which comprises:
a setting module for executing step S1: presetting an initial relative coordinate between a piloting robot and a following robot;
a pilot obtaining module, configured to perform step S2: acquiring current coordinates and current speed information of the piloting robot;
a desired calculation module for performing step S3: calculating desired coordinates, a desired orientation angle, and desired speed information of the following robot based on the initial relative coordinates, the current coordinates of the piloting robot, and the current speed information;
a follow acquisition module, configured to perform step S4: acquiring the current coordinate and the current orientation angle of the following robot;
a coordinate deviation calculation module for executing step S5: calculating a coordinate deviation of the following robot based on the current coordinates and the expected coordinates of the following robot;
an orientation deviation calculation module, configured to execute step S6: calculating an orientation deviation of the following robot based on a current orientation angle and a desired orientation angle of the following robot;
an output calculation module, configured to execute step S7: calculating output speed information of the following robot based on the coordinate deviation, the orientation deviation and the expected speed information of the following robot;
a control module for performing step S8: and controlling the following robot to move based on the output speed information of the following robot.
Wherein the desired computation module comprises:
a desired coordinate unit for performing step S31: calculating an expected coordinate of the following robot based on the initial relative coordinate and a current coordinate of a piloting robot;
a turning radius unit for executing step S32: the system comprises a control unit, a control unit and a control unit, wherein the control unit is used for calculating the turning radius of the piloting robot based on the current angular velocity and the current linear velocity of the piloting robot;
a desired orientation angle unit for performing step S33: calculating a desired heading angle of the following robot based on the initial relative coordinates and a turning radius of the piloting robot;
a desired angular velocity unit for performing step S34: calculating an expected angular velocity of the following robot based on the current angular velocity of the piloted robot;
a desired linear velocity unit for performing step S35: calculating a desired linear velocity of the following robot based on the initial relative coordinates, and a current angular velocity, a current linear velocity, and a turning radius of the piloting robot.
An output calculation module comprising:
an output line speed unit for performing step S71: calculating an output linear velocity of the following robot based on the desired linear velocity and the front-to-back deviation of the following robot;
an output angular velocity unit for executing step S72: calculating an output angular velocity of the following robot based on the desired angular velocity, the left-right deviation, and the orientation deviation of the following robot.
The control module is also used for triggering the pilot acquisition module.
All relevant contents of the steps related to the foregoing embodiments of the control device may be referred to the corresponding descriptions, and are not described herein again.
In this embodiment, the control device is divided into functional modules according to the above method example, and the functional modules may be implemented in a form of hardware or may be implemented by hardware executing corresponding software. Whether a function is performed as hardware or computer software drives hardware depends upon the particular application and design constraints imposed on the solution. Skilled artisans may implement the described functionality in varying ways for each particular application, in conjunction with the embodiments, but such implementation decisions should not be interpreted as causing a departure from the scope of the present application.
It should be noted that the division of the modules in this embodiment is schematic, and is only a logic function division, and there may be another division manner in actual implementation. For example, the functional blocks may be divided for the respective functions, or two or more functions may be integrated into one processing block.
The present embodiment also provides a computer storage medium, in which computer instructions are stored, and when the computer instructions are run on an electronic device, the electronic device is caused to execute the foregoing control method.
The integrated functional units, if implemented in the form of software functional units and sold or used as separate products, may be stored in a readable storage medium. Based on such understanding, the technical solutions of the embodiments of the present application may be essentially or partially contributed to by the prior art, or all or part of the technical solutions may be embodied in the form of a software product, where the software product is stored in a storage medium and includes several instructions to enable a device (which may be a single chip, a chip, or the like) or a processor (processor) to execute all or part of the steps of the methods of the embodiments of the present application. And the aforementioned storage medium includes: various media capable of storing program codes, such as a usb disk, a removable hard disk, a Read Only Memory (ROM), a Random Access Memory (RAM), a magnetic disk, or an optical disk.
The control device and the computer storage medium provided by the present invention are both used for executing the corresponding methods provided in the foregoing, and therefore, the beneficial effects achieved by the control device and the computer storage medium can refer to the beneficial effects in the corresponding methods provided in the foregoing, and are not described herein again.
The above-mentioned embodiments are only used for illustrating the technical solutions of the present invention, and not for limiting the same; although the present invention has been described in detail with reference to the foregoing embodiments, it will be understood by those of ordinary skill in the art that: the technical solutions described in the foregoing embodiments may still be modified, or some technical features may be equivalently replaced; and such modifications or substitutions do not depart from the spirit and scope of the corresponding technical solutions of the embodiments of the present invention.
Claims (10)
1. A multi-machine formation control method based on piloting following is characterized by comprising the following steps:
presetting an initial relative coordinate between a piloting robot and a following robot;
acquiring current coordinates and current speed information of the piloting robot;
calculating desired coordinates, a desired orientation angle, and desired speed information of the following robot based on the initial relative coordinates, the current coordinates of the piloting robot, and the current speed information;
acquiring the current coordinate and the current orientation angle of the following robot;
calculating a coordinate deviation of the following robot based on the current coordinate and the desired coordinate of the following robot;
calculating an orientation deviation of the following robot based on a current orientation angle and a desired orientation angle of the following robot;
calculating output speed information of the following robot based on the coordinate deviation, the orientation deviation and the expected speed information of the following robot;
and controlling the following robot to move based on the output speed information of the following robot.
2. The multi-machine formation control method based on pilot following according to claim 1, wherein the current speed information of the pilot robot comprises a current angular speed and a current linear speed, the desired speed information of the following robot comprises a desired angular speed and a desired linear speed, and the output speed information of the following robot comprises an output angular speed and an output linear speed;
the coordinate deviation of the following robot comprises left and right deviation and front and back deviation;
the steps are as follows: calculating expected coordinates, an expected orientation angle and expected speed information of the following robot based on the initial relative coordinates, the current coordinates of the piloting robot and the current speed information, and specifically comprising the following steps of:
calculating an expected coordinate of the following robot based on the initial relative coordinate and a current coordinate of a piloting robot;
calculating the turning radius of the piloting robot based on the current angular velocity and the current linear velocity of the piloting robot;
calculating a desired heading angle of the following robot based on the initial relative coordinates and a turning radius of the piloting robot;
calculating an expected angular velocity of the following robot based on the current angular velocity of the piloted robot;
calculating an expected linear velocity of the following robot based on the initial relative coordinates, and a current angular velocity, a current linear velocity, and a turning radius of the piloting robot;
the steps are as follows: calculating output speed information of the following robot based on the coordinate deviation, the orientation deviation and the expected speed information of the following robot, and specifically comprising:
calculating an output linear velocity of the following robot based on the desired linear velocity and the front-to-back deviation of the following robot;
calculating an output angular velocity of the following robot based on the desired angular velocity, the left-right deviation, and the orientation deviation of the following robot.
3. The pilot following-based multi-machine formation control method according to claim 2, wherein the expected coordinates of the following robot are expressed as (x)r，yr) Calculated according to the following formula:
xr＝x0+a；
yr＝y0+b；
wherein (a, b) are the initial relative coordinates, (x)0，y0) The current coordinate of the piloting robot is obtained;
the turning radius of the piloted robot is represented as r0Calculated according to the following formula:
wherein v is0Is the current linear velocity, w, of the piloting robot0The current angular velocity of the piloted robot;
the desired orientation angle of the following robot is denoted as θrCalculated according to the following formula:
the desired angular velocity of the following robot is denoted wrCalculated according to the following formula:
wr＝w0；
the desired linear velocity of the following robot is denoted vrCalculated according to the following formula:
wherein, the wtIs a preset angular velocity threshold and is a positive value;
the steps are as follows: acquiring current coordinates and current speed information of the piloting robot, wherein the method also comprises the following steps: a preset angular velocity threshold.
4. The pilot following-based multi-machine formation control method according to claim 3, wherein a left-right deviation of the following robot is represented by x, a front-back deviation is represented by y, and the following formula is calculated as follows:
x＝x1-xr；
y＝y1-yr；
wherein (x)1，y1) Is the current coordinates of the following robot;
the orientation deviation of the following robot is represented as theta and is calculated according to the following formula:
θ＝θ1-θr；
wherein, theta1Is the current orientation angle of the following robot.
The output linear velocity of the following robot is denoted by veCalculated according to the following formula:
ve＝vr+k1y；
wherein k is1Is a preset constant and is a negative value;
the output angular velocity of the following robot is denoted weCalculated according to the following formula:
we＝wr+f(k2)x+k3θ；
wherein k is2And k3Are all preset constants and positive values.
5. The pilot following-based multi-machine formation control method according to claim 1, wherein the steps of: controlling the following robot to move based on the output speed information of the following robot, and then further comprising:
returning to the step: and acquiring the current coordinate and the current speed information of the piloting robot.
6. The multi-machine formation control method based on pilot following according to claim 1, wherein a pilot laser radar and a pilot encoder are installed on the pilot robot, the current coordinate of the pilot robot is output after being collected by the pilot laser radar, and the current speed information of the pilot robot is output after being collected by the pilot encoder;
and the following laser radar is installed on the following robot, and the current coordinate and the current orientation angle of the following robot are both acquired by the following laser radar and then output.
7. A multimachine formation controlling means based on pilot is followed, its characterized in that includes:
the setting module is used for presetting an initial relative coordinate between the piloting robot and the following robot;
the navigation acquisition module is used for acquiring the current coordinate and the current speed information of the navigation robot;
an expected calculation module for calculating expected coordinates, an expected orientation angle and expected speed information of the following robot based on the initial relative coordinates, the current coordinates of the piloting robot and the current speed information;
the following acquisition module is used for acquiring the current coordinate and the current orientation angle of the following robot;
a coordinate deviation calculation module for calculating a coordinate deviation of the following robot based on a current coordinate and an expected coordinate of the following robot;
an orientation deviation calculation module for calculating an orientation deviation of the following robot based on a current orientation angle and a desired orientation angle of the following robot;
the output calculation module is used for calculating the output speed information of the following robot based on the coordinate deviation, the orientation deviation and the expected speed information of the following robot;
and the control module is used for controlling the following robot to move based on the output speed information of the following robot.
8. The multi-machine formation control device according to claim 7, wherein the current speed information of the piloting robot includes a current angular speed and a current linear speed, the desired speed information of the following robot includes a desired angular speed and a desired linear speed, and the output speed information of the following robot includes an output angular speed and an output linear speed;
the coordinate deviation of the following robot comprises left and right deviation and front and back deviation;
the expectation calculation module includes:
an expected coordinate unit for calculating an expected coordinate of the following robot based on the initial relative coordinate and a current coordinate of a piloting robot;
a turning radius unit for calculating a turning radius of the piloted robot based on the current angular velocity and the current linear velocity of the piloted robot;
a desired orientation angle unit for calculating a desired orientation angle of the following robot based on the initial relative coordinates and a turning radius of the piloting robot;
an expected angular velocity unit for calculating an expected angular velocity of the following robot based on a current angular velocity of the piloting robot;
an expected linear velocity unit for calculating an expected linear velocity of the following robot based on the initial relative coordinates, and the current angular velocity, the current linear velocity, and the turning radius of the piloting robot;
the output calculation module includes:
an output linear velocity unit for calculating an output linear velocity of the following robot based on a desired linear velocity and a front-to-back deviation of the following robot;
an output angular velocity unit for calculating an output angular velocity of the following robot based on a desired angular velocity, a left-right deviation, and an orientation deviation of the following robot;
the control module is also used for triggering the navigation acquisition module;
the navigation robot is provided with a navigation laser radar and a navigation encoder, the navigation laser radar is used for outputting the current coordinate of the navigation robot after being collected, the navigation encoder is used for outputting the current speed information of the navigation robot after being collected, and the navigation acquisition module is electrically connected with the navigation laser radar and the navigation encoder;
follow the laser radar of installing on the robot, follow the laser radar and be used for gathering the back output follow the current coordinate and the current orientation angle of robot, follow and acquire the module electricity and connect follow the laser radar.
9. The multi-machine formation control device according to claim 8, wherein the desired coordinates of the following robots are expressed as (x)r，yr) Calculated according to the following formula:
xr＝x0+a；
yr＝y0+b；
wherein (a, b) are the initial relative coordinates, (x)0，y0) The current coordinate of the piloting robot is obtained;
the turning radius of the piloting robot is represented as r0Calculated according to the following formula:
wherein v is0Is the current linear velocity, w, of the piloting robot0The current angular velocity of the piloted robot;
the desired orientation angle of the following robot is denoted as θrCalculated according to the following formula:
the desired angular velocity of the following robot is denoted wrCalculated according to the following formula:
wr＝w0；
the desired linear velocity of the following robot is denoted vrCalculated according to the following formula:
wherein, the wtIs a preset angular velocity threshold and is a positive value;
the setting module is also used for presetting an angular speed threshold;
the left deviation and the right deviation of the following robot are expressed as x, the front deviation and the rear deviation are expressed as y, and the following formula is calculated:
x＝x1-xr；
y＝y1-yr；
wherein (x)1，y1) Is the current coordinates of the following robot;
the orientation deviation of the following robot is represented as theta and is calculated according to the following formula:
θ＝θ1-θr；
wherein, theta1Is the current orientation angle of the following robot.
The output linear velocity of the following robot is denoted veCalculated according to the following formula:
ve＝vr+k1y；
wherein k is1Is a preset constant and is a negative value;
the output angular velocity of the following robot is denoted weCalculated according to the following formula:
we＝wr+f(k2)x+k3θ；
wherein k is2And k3Are all preset constants and positive values.
10. A computer-readable storage medium having stored thereon computer instructions which, when run on a processor of an electronic device, cause the electronic device to perform the method of any one of claims 1-6.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210444690.4A CN114740863A (en) | 2022-04-26 | 2022-04-26 | Multi-machine formation control method and device based on piloting following |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210444690.4A CN114740863A (en) | 2022-04-26 | 2022-04-26 | Multi-machine formation control method and device based on piloting following |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN114740863A true CN114740863A (en) | 2022-07-12 |
Family
ID=82283960
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210444690.4A Pending CN114740863A (en) | 2022-04-26 | 2022-04-26 | Multi-machine formation control method and device based on piloting following |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114740863A (en) |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116149340A (en) * | 2023-04-23 | 2023-05-23 | 季华实验室 | Differential wheel chassis robot self-adaptive path following method and related equipment thereof |
| WO2024179000A1 (en) * | 2023-02-28 | 2024-09-06 | 华为技术有限公司 | Vehicle control method and related apparatus |
Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| MX2010012981A (en) * | 2010-11-25 | 2012-05-25 | Jose Ignacio Huertas Cardozo | Non-invasive mechatronic device for measuring the linear speed in vehicles. |
| 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 |
| CN109782759A (en) * | 2019-01-08 | 2019-05-21 | 华侨大学 | A kind of Approximate Decoupling of wheeled mobile robot, quick Trajectory Tracking Control method |
| CN110597268A (en) * | 2019-09-30 | 2019-12-20 | 东南大学 | Wheel type mobile robot track tracking control method based on cascade system theory |
| CN111399500A (en) * | 2020-01-19 | 2020-07-10 | 浙江工业大学 | Centralized formation control method for two-wheeled self-balancing vehicle |
| CN111880557A (en) * | 2020-07-02 | 2020-11-03 | 中国人民解放军军事科学院国防科技创新研究院 | Unmanned aerial vehicle cluster formation flight control method and device |
| CN113093804A (en) * | 2021-04-06 | 2021-07-09 | 上海海事大学 | Unmanned ship formation control method and control system based on inversion sliding mode control |
-
2022
- 2022-04-26 CN CN202210444690.4A patent/CN114740863A/en active Pending
Patent Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| MX2010012981A (en) * | 2010-11-25 | 2012-05-25 | Jose Ignacio Huertas Cardozo | Non-invasive mechatronic device for measuring the linear speed in vehicles. |
| 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 |
| CN109782759A (en) * | 2019-01-08 | 2019-05-21 | 华侨大学 | A kind of Approximate Decoupling of wheeled mobile robot, quick Trajectory Tracking Control method |
| CN110597268A (en) * | 2019-09-30 | 2019-12-20 | 东南大学 | Wheel type mobile robot track tracking control method based on cascade system theory |
| CN111399500A (en) * | 2020-01-19 | 2020-07-10 | 浙江工业大学 | Centralized formation control method for two-wheeled self-balancing vehicle |
| CN111880557A (en) * | 2020-07-02 | 2020-11-03 | 中国人民解放军军事科学院国防科技创新研究院 | Unmanned aerial vehicle cluster formation flight control method and device |
| CN113093804A (en) * | 2021-04-06 | 2021-07-09 | 上海海事大学 | Unmanned ship formation control method and control system based on inversion sliding mode control |
Non-Patent Citations (2)
| Title |
|---|
| 张文霞;袁健;: "反步轨迹跟踪控制的移动机器人队形成形控制", 自动化与仪表, no. 04, 15 April 2016 (2016-04-15) * |
| 陈垦;刘涛;杜新光;林强;刘双;: "基于自适应神经网络滑模控制的常压潜水装具航向控制器设计", 中国造船, no. 01, 30 March 2018 (2018-03-30) * |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2024179000A1 (en) * | 2023-02-28 | 2024-09-06 | 华为技术有限公司 | Vehicle control method and related apparatus |
| CN116149340A (en) * | 2023-04-23 | 2023-05-23 | 季华实验室 | Differential wheel chassis robot self-adaptive path following method and related equipment thereof |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN114740863A (en) | Multi-machine formation control method and device based on piloting following | |
| EP3763589A1 (en) | Methods, apparatuses, and media for autonomously driving vehicle | |
| US8958976B2 (en) | Traveling apparatus, control method therefor, and control program | |
| WO2022105393A1 (en) | Method, system, and device for controlling driving direction of vehicle | |
| JP2669043B2 (en) | Autonomous vehicles | |
| CN111196271A (en) | Automatic parking method, device, equipment and storage medium | |
| CN111267857B (en) | Lane changing track generation method and device for automatic driving vehicle | |
| CN109375624B (en) | Double-steering-wheel AGV arc path generation method and device and medium | |
| CN114148320B (en) | Path tracking control method and equipment | |
| CN113492907B (en) | Lane keeping method and system based on multipoint preview | |
| CN113093804B (en) | Unmanned ship formation control method and control system based on inversion sliding mode control | |
| CN110789530B (en) | Four-wheel independent steering-independent driving vehicle trajectory tracking method and system | |
| US11372414B2 (en) | Robotic motion control method and apparatus and robot using the same | |
| CN112109732A (en) | Intelligent driving self-adaptive curve pre-aiming method | |
| CN111522351A (en) | Three-dimensional formation and obstacle avoidance method for underwater robot | |
| CN116985136B (en) | Quaternion-based mechanical arm node attitude speed look-ahead control method and device | |
| CN111830832B (en) | Bionic gliding machine dolphin plane path tracking method and system | |
| Yoshimoto et al. | OP-AmP 2017 team description paper | |
| CN111674406A (en) | Method for controlling vehicle transverse direction of automatic driving system | |
| CN110682910A (en) | Bend auxiliary driving method and device | |
| CN115562304A (en) | Mobile robot visual servo control method and system based on depth point cloud | |
| CN114019962A (en) | Vehicle lane change control method and device and vehicle | |
| CN115848404A (en) | Articulated vehicle control method and articulated vehicle | |
| CN114545936B (en) | Robot path optimization method and system based on obstacle avoidance planning | |
| CN203643841U (en) | Two-wheel micro-mouse fast diagonal sprint system based on ARM9 |
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 |