CN113485354A - Heterogeneous multi-robot positioning and control method based on orchard map - Google Patents

Heterogeneous multi-robot positioning and control method based on orchard map Download PDF

Info

Publication number
CN113485354A
CN113485354A CN202110840772.6A CN202110840772A CN113485354A CN 113485354 A CN113485354 A CN 113485354A CN 202110840772 A CN202110840772 A CN 202110840772A CN 113485354 A CN113485354 A CN 113485354A
Authority
CN
China
Prior art keywords
robot
tree
gnss
following
orchard
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.)
Granted
Application number
CN202110840772.6A
Other languages
Chinese (zh)
Other versions
CN113485354B (en
Inventor
毛文菊
刘恒
杨福增
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Northwest A&F University
Original Assignee
Northwest A&F University
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Northwest A&F University filed Critical Northwest A&F University
Priority to CN202110840772.6A priority Critical patent/CN113485354B/en
Publication of CN113485354A publication Critical patent/CN113485354A/en
Application granted granted Critical
Publication of CN113485354B publication Critical patent/CN113485354B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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/0276—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle
    • 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

  • Engineering & Computer Science (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
  • Manipulator (AREA)

Abstract

The invention discloses a heterogeneous multi-robot positioning and control method based on an orchard map, which specifically comprises the following steps: a complete orchard map is constructed by a robot, and then the central points of all fruit trees in the orchard map are converted into GNSS absolute coordinate values [ tree ] through GNSS coordinate system conversioni_gnss_x,treei_gnss_y](ii) a The navigation robot can estimate the nearest fruit tree coordinate [ tree ] in the range according to the radar scanning angle and the relative distancen_gnss_x,treen_gnss_y]And the real coordinates of the tree are compared with the real coordinates of each fruit tree in the orchard mapi_gnss_x,treei_gnss_y]Comparison, when the distance difference dminAnd when the minimum time is reached, judging the fruit tree GNSS real coordinate closest to the robot, and finally pre-estimating the accurate positions and the directions of the multiple robots in the orchard according to the GNSS real coordinate of the fruit tree to finish the inter-row operation and the head-of-the-ground turning operation of the orchard. The invention utilizes the advantages of orchard map and different navigation modes of various robots on the basis of the collaborative operation of the navigation-following heterogeneous robots, and is accurateAnd the operation positions of the multiple robots in the orchard are positioned, so that the multiple robots can cooperatively operate in the orchard.

Description

Heterogeneous multi-robot positioning and control method based on orchard map
Technical Field
The invention belongs to the field of orchard machinery, and particularly relates to a heterogeneous multi-robot autonomous navigation control method based on an orchard map.
Background
With the mode of intensive planting and operation adopted by more and more orchards, the single-machine operation cannot meet the requirement of seasonal production of the orchards, and the adoption of multi-machine operation becomes an inevitable trend. However, due to different manufacturers, the existing orchard robots have different accuracies of sensors and controllers equipped in the robots, and different autonomous navigation modes of the robots, and it is difficult to construct the heterogeneous robots into a multi-robot group. And purchasing a new sensor or robot increases the production cost and is not beneficial to maintenance.
At present, heterogeneous multi-robot cooperative positioning and control are researched mostly in a GNSS-free indoor environment, the positions of opposite sides are mutually observed through relative positioning sensors on mobile robots, then the information of the sensors of the robots is fused by using a particle filter or extended Kalman filter algorithm, the position and the direction of the robots at the next moment are pre-estimated, and finally, the heterogeneous multi-robots cooperatively work according to a fixed formation. In an orchard environment, due to the shielding of fruit trees, the robot cannot directly observe the positions of other robots through relative positioning sensors, cooperative work needs to be completed by means of GNSS absolute information, the orchard robot is provided with the GNSS sensors with different precision, accurate positioning is difficult to obtain during inter-row work of the fruit trees, cooperative work cannot be completed, and the problems of positioning precision and incapability of cooperative control exist.
Disclosure of Invention
The method aims to solve the problems that in an orchard environment, heterogeneous multi-robot GNSS sensors are different in accuracy and positioning and control of robots are difficult to achieve. The robot network has more than 3 heterogeneous robots, GNSS sensors of each robot are different in accuracy, autonomous navigation modes of the robots in an orchard are different, a navigation robot depends on radar navigation, a following robot depends on GNSS navigation, and accurate movement cannot be achieved when multiple robots are constructed. The invention provides a heterogeneous multi-robot positioning and control method based on an orchard map, which adopts the technical scheme that:
a heterogeneous multi-robot positioning and control method based on an orchard map is characterized by comprising the following steps:
the piloting robot is remotely controlled by a person to walk in a fruit tree row, a complete orchard map is firstly constructed, and then through GNSS coordinate system conversion, all fruit tree center points in the orchard map are converted into GNSS absolute coordinate values [ tree ]i_gnss_x,treei_gnss_y](ii) a The navigation robot can estimate the nearest fruit tree coordinate [ tree ] in the range according to the radar scanning angle and the relative distancen_gnss_x,treen_gnss_y]By formula (1) and the coordinates [ tree ] of each fruit tree in the orchard mapi_gnss_x,treei_gnss_y]Comparison, when dminWhen the GNSS real coordinate is close to 0, the GNSS real coordinate of the fruit tree is obtained;
further, the real coordinate [ tree ] of the fruit tree closest to the roboti_gnss_x,treei_gnss_y]The decision formula of (2) is:
Figure BDA0003178859410000021
estimating a reference navigation line equation of autonomous navigation of the piloting robot and the following robots 1 and 2 and an ideal position coordinate [ X ] of the robot at the next moment according to the needed real coordinates of the fruit trees, the row spacing r _ s and the planting spacing p _ s1,Y1]，[X_f1,Y_f1]，[X_f2,Y_f2]；
Further, an ideal position coordinate [ X ] of the piloted robot1,Y1]The calculation formula is as follows:
Figure BDA0003178859410000022
further, a specific calculation formula of a reference navigation line of the piloting robot is as follows:
Aj＝treei+1_gnss_y-treei_gnss_y
Bj＝treei_gnss_x-treei+1_gnss_x
Cj＝(treei+1_gnss_x-treei_gnss_y)-(treei_gnss_x-treei+1_gnss_y) (3)
A1Xi+B1Yi+C1＝0 (4)
further, the ideal position coordinate [ X _ f ] of the robot 1 is followed1,Y_f1]The calculation formula is as follows:
Figure BDA0003178859410000023
further, the ideal position coordinate [ X _ f ] of the robot 2 is followed2,Y_f2]The calculation formula is as follows:
Figure BDA0003178859410000024
further, substituting the ideal position coordinates of the following robot 2 in the nearer tree into equation (3) yields the reference course of the following robot 2:
A2Xi+B2Yi+C2＝0 (7)
and finally, calculating the transverse deviation and the course deviation from the ideal and actual position coordinates of each robot to the reference navigation line respectively, and enabling each robot to reach the ideal position by adjusting the advancing angular speed and linear speed of each robot in real time, thereby finishing the inter-row operation.
When the piloting robot starts to turn clockwise by 90 degrees, the heading turning operation is started, the piloting robot calculates the reference navigation line 1 and the ideal position coordinates at the next moment according to the real coordinates of the fruit trees, the position coordinates and the reference navigation line 1 are sent to the following robots 1 and 2 through communication, the following robots 1 and 2 judge the effectiveness of the ideal position coordinates, the transverse deviation and the course deviation from the ideal position coordinates and the actual position coordinates to the reference navigation line are calculated, the ideal position is reached through real-time adjustment of the advancing angular speed and the linear speed of each robot, and the heading operation is finished.
The invention has the beneficial effects that: in an orchard working environment, the established orchard map and relative position conversion are utilized to identify the GNSS real coordinate of the fruit tree coordinate closest to the robot, and the accurate position and course of the multiple robots in the orchard are pre-estimated through the GNSS real coordinate of the fruit tree, so that the working precision can be improved, the walking path of the robots can be planned in real time, and finally the cooperative work of the heterogeneous multiple robots in the orchard according to the fixed formation is realized.
Drawings
The invention is further described with reference to the accompanying drawings and specific embodiments:
FIG. 1 is a schematic diagram of the orchard map based heterogeneous multi-robot positioning and control method of the present invention;
FIG. 2 is a simplified inter-row queue control diagram of the orchard map based heterogeneous multi-robot positioning and control method of the present invention;
FIG. 3 is a flow chart of the inter-row queue control of the orchard map based heterogeneous multi-robot positioning and control method of the present invention;
FIG. 4 is a head-of-ground turn queue control diagram of the orchard map-based heterogeneous multi-robot positioning and control method of the present invention;
fig. 5 is a flow chart of turning control of the ground of the heterogeneous multi-robot positioning and control method based on the orchard map.
Detailed Description
The invention is described in further detail below with reference to the accompanying drawings:
as shown in fig. 1, a piloting robot is remotely controlled by a person to walk in a fruit tree row, and an orchard map is constructed by scanning and recording an orchard through a 3D radar; converting all fruit tree center points in the orchard map into GNSS absolute coordinate values through the built GNSS base station; the navigation robot selects a fruit tree point closest to the radar scanning angle, and the GNSS accurate coordinate of the fruit tree point closest to the fruit tree point is obtained by comparing the fruit tree point closest to the radar scanning angle with the center points of all fruit trees in the orchard map, so that the current absolute coordinate of the navigation robot is determined; forming a reference navigation line for autonomous navigation of the piloting robot and an ideal position of the piloting robot at the next moment according to the scanned central point of the fruit tree; calculating the transverse deviation from the piloting robot to a reference navigation line, and adjusting the transverse deviation and the course of the piloting robot to reach an ideal position in real time; referring to the GNSS coordinates of the tree, the ideal position of the following robot is sent to the following robots 1 and 2; and the following robots 1 and 2 adjust the actual positions of the following robots in real time according to the ideal positions sent by the piloting robot.
As shown in figure 2, the pilot robot, the following robot 1 and the following robot 2 are arranged in a line-shaped queue for line-to-line operation, and all fruit tree coordinates in the apple orchard are used as [ tree ]i_gnss_x,treei_gnss_y]Indicating the robot's own GNSS position coordinates [ X ]i,Yi]Relative coordinates scanned by the 3D radar and estimated fruit tree coordinatesn_gnss_x,treen_gnss_y]Uniformly expressing; the piloting robot is centered at 270 degrees and 360 degrees]、[0°,90°]And [90 °,180 ° ]]Tree 1 coordinate [ X ] estimated by piloting robot within scanning range of the nearest distancei+Δx1,Yi+Δy1]Tree 2 coordinate [ X ]i+Δx2,Yi++Δy2]Tree 3 coordinate [ X ]i+Δx3,Yi++Δy3]Substituting into equation (1) [ treen_gnss_x,treen_gnss_y]With some fruit tree coordinatesi_gnss_x,treei_gnss_y]Comparison, when dminIf it is close to 0, the actual position coordinate [ tree ] of tree 1 is obtained1_gnss_x,tree1_gnss_y]Actual position coordinates of Tree 22_gnss_x,tree2_gnss_y]Actual position coordinates of Tree 33_gnss_x,tree3_gnss_y]；
Then, according to the actual position coordinates of tree 2, adding row spacing r _ s and plant spacing p _ s to obtain the estimated position coordinates [ tree ] of tree 42_gnss_x+r_s,treen_gnss_y+p_s]Estimated position coordinates of Tree 5 [ tree ]2_gnss_x+r_s,treen_gnss_y]Substituting into equation (1) [ treen_gnss_x,treen_gnss_y]With some fruit tree coordinatesi_gnss_x,treei_gnss_y]Comparison, when dminWhen the coordinate is close to 0, the actual position coordinates of tree 4 and tree 5 are [ tree ]4_gnss_x,tree4_gnss_y]，[tree5_gnss_x,tree5_gnss_y]；
Obtaining next time of the piloting robot, the following robot 1 and the following robot 2 through the actual position coordinates of the tree and the formulas (2), (4) and (5)Ideal position [ X1,Y1]，[X_f1,Y_f1]，[X_f2,Y_f2]；
Meanwhile, a linear equation of the reference navigation line 1 is obtained through formula 3 according to the actual position coordinates of the tree 1 and the tree 2
A1Xi+B1Yi+C1＝0 (4)
And (3) bringing the actual position coordinates of the trees 4 and 5 into a linear equation of the reference navigation line 2:
A2Xi+B2Yi+C2＝0 (7)
obtaining the accurate actual position [ X ] of the piloting robot 1 by the formula (8)i,Yi]From the ideal position [ X ]1,Y1]Obtaining S from the distance of the formula (9), obtaining the transverse distances d _ l and d of the ideal position and the actual position of the pilot robot by using the formulas (10) and (11), obtaining the transverse deviation delta d of the current position and the ideal position of the pilot robot and the reference navigation line 1 by using the formulas (12), (13) and (14), and advancing to Heading of the ideal position, wherein the Heading deviation is as follows:
[Xi,Yi]＝[tree2_gnss_x-Δx2,tree2_gnss_y-Δy2] (8)
Figure BDA0003178859410000051
Figure BDA0003178859410000052
Figure BDA0003178859410000053
Δd＝d_l-d (12)
Figure BDA0003178859410000054
Heading_error＝Headingi-current_yaw (14)
passing formula (15) and actual position [ X _ f ] of following robot 1i1,Y_fi1]And ideal position [ X _ f [ ]1,Y_f1]The distance of (2) is obtained as S1, equations (16) and (17), and the actual position [ X _ f ] of the following robot 1i1,Y_fi1]Ideal position [ X _ f [ ]1,Y_f1]The transverse distances from the reference navigation line 1 are d1 and d _ l 1; equations (18), (19) and (13) follow the lateral deviation delta d1, Heading1 and Heading deviation of the ideal position of the robot 1 from the actual position;
Figure BDA0003178859410000055
Figure BDA0003178859410000056
Figure BDA0003178859410000057
Δd1＝d_l1-d1 (18)
Figure BDA0003178859410000058
passing formula (20) and actual position [ X _ f ] of following robot 2i2,Y_fi2]Ideal position [ X _ f2，Y_f2]The distance of (2) is obtained as S2 by the equations (21) and (22) and the ideal position of the following robot 2
Figure BDA0003178859410000065
And the actual position [ X _ fi2,Y_fi2]The transverse distance from the reference navigation line 2 is d _ l2 and d, and the transverse deviation delta d2, Heading2 and Heading deviation between the ideal position of the following robot 2 and the actual position are obtained through the formulas (23), (24) and (13);
Figure BDA0003178859410000061
Figure BDA0003178859410000062
Figure BDA0003178859410000063
Δd2＝d_l2-d2 (23)
Figure BDA0003178859410000064
as shown in fig. 3, the piloting robot, the following robot 1 and the following robot 2 are arranged in a line-shaped queue to operate between rows, an orchard map containing the absolute position of the center of a trunk is extracted by the piloting robot, radar navigation is started, absolute coordinates of the tree 1, the tree 2 and the tree 3 in the nearest scanning ranges of [270 degrees, 360 degrees ], [0 degrees, 90 degrees ] and [90 degrees, 180 degrees ] of the piloting robot 1 are obtained through judgment of formula (1), meanwhile, the advancing pose of the piloting robot at the next moment is obtained according to the actual trunk coordinates of [270 degrees, 360 degrees ], [0 degrees, 90 degrees ], and the ideal position coordinates and the coordinates of the tree 2-tree 5 of the following robot 1 and the following robot 2 at the next moment are sent to the following robots 1 and 2; judging whether the current course deviation is within a set range, adjusting the angular speed of the robot when the current course deviation is out of the range, otherwise, continuously judging whether the current transverse deviation is within the set range, if not, adjusting the angular speed and the linear speed at the same time, otherwise, only adjusting the linear speed;
after receiving the tree 2, the tree 3 and the ideal position coordinates at the next moment, the following robot 1 respectively calculates a linear equation of a reference navigation line 1, a transverse distance d _ l1, a transverse distance d1, a linear distance S1, a transverse deviation delta d1 and Heading; by judging whether the current course deviation is within a set range, adjusting the angular speed of the robot when the current course deviation is out of the range, otherwise, continuously judging whether the current transverse deviation is within the set range, if not, adjusting the angular speed and the linear speed at the same time, otherwise, only adjusting the linear speed; and (3) stopping moving along with the robot 1 until the piloting robot reaches the ground and starts to turn at right angle, and waiting for a ground turning operation command.
After receiving the tree 4, the tree 5 and the ideal position coordinates at the next moment, the following robot 2 respectively calculates a linear equation of a reference navigation line 2, a transverse distance d _ l2, a transverse distance d2, a linear distance S2, a transverse deviation delta d2 and a Heading 1; by judging whether the current course deviation is within a set range, adjusting the angular speed of the robot when the current course deviation is out of the range, otherwise, continuously judging whether the current transverse deviation is within the set range, if not, adjusting the angular speed and the linear speed at the same time, otherwise, only adjusting the linear speed; until the following robot 2 reaches a point parallel to the following robot 1, the following robot 2 stops moving, and waits for a head-of-ground turning work command.
As shown in fig. 4, the equation of the reference navigation line 1 is obtained from the coordinates of the trees 1 and 2 and the equations (1), (2) and (5), and as the piloting robot advances, the ideal position coordinate [ X ] of the piloting point 1 at the next moment is continuously generated by the equation (2)1,Y1]Sending the coordinates of the following moment of the piloting robot to the following robot 1 and the following robot 2; the following robot 1 waits until the piloting robot runs to the 3 rd ideal position point, the following robot 1 reaches the 1 st point issued by the piloting robot, and the following robot stops moving after turning clockwise by 90 degrees; the following robot 2 waits for the following robot 1 to reach the 3 rd ideal position point issued by the piloting robot, the following robot 2 reaches the 2 nd point issued by the piloting robot, and the following robot stops moving after turning clockwise by 90 degrees; when the piloting robot reaches the 5 th ideal position coordinate, the following robot 1 starts to travel to the 4 th ideal position coordinate point and starts to turn clockwise by 90 degrees and then stops moving, and the following robot 2 can travel to the 3 rd ideal position coordinate point and starts to turn clockwise by 90 degrees and then stops moving; and when the three robots all reach the target point, the turning operation of the ground head is finished.
When the piloting robot reaches the turning point at the ground and starts turning 90 degrees clockwise as shown in fig. 5, the multi-robot ground turning operation starts and the piloting robot starts to turn at the ground turning pointThe navigation robot judges [270 degrees, 360 degrees ] according to the formula (1)]、[0°,90°]And [90 °,180 ° ]]The real coordinate values of the trees 1, 2 and 3 in the nearest range are calculated to calculate the current position of the piloting robot and the ideal position coordinate [ X ] of the piloting robot at the next moment1，Y1]Judging whether the current course deviation is in a set range or not, if so, continuously judging whether the current transverse deviation is in the set range or not, otherwise, simultaneously adjusting the angular speed and the linear speed, otherwise, only adjusting the linear speed; meanwhile, every time the piloting robot advances to the ideal position, the calculated ideal position at the next moment is sent to the following robots 1 and 2;
when the pilot robot sends the 3 rd position point, the following robot 1 advances to the 1 st position point sent by the pilot robot and starts to turn clockwise by 90 degrees, after the movement is finished, the following robot continues to advance according to the ideal position sent by the pilot robot, whether the current course deviation is within a set range is judged in the advancing process, when the course deviation is out of the set range, the angular speed of the robot is adjusted, otherwise, the angular speed and the linear speed are continuously judged, if the course deviation is not within the set range, the angular speed and the linear speed are adjusted at the same time, otherwise, only the linear speed is adjusted until the 4 th ideal position point sent by the pilot robot is reached and the 90-degree clockwise steering is started;
when the follower robot 1 reaches the 3 rd position point, the follower robot 2 advances to the position point sent by the 2 nd pilot robot, and starts turning clockwise by 90 degrees, after finishing the movement, the follower robot continues to advance according to the ideal position sent by the pilot robot, judges whether the current course deviation is in the set range in the advancing process, adjusts the angular speed of the robot when the course deviation is out of the set range, otherwise, continuously judges whether the current transverse deviation is in the set range, if not, simultaneously adjusts the angular speed and the linear speed, otherwise, only adjusts the linear speed until reaching the 3 rd ideal position point sent by the pilot robot and starting turning clockwise by 90 degrees, if so, the ground turning operation is finished, otherwise, continuously adjusts.
The present invention is not limited to the above embodiments, and any modifications, substitutions, and improvements made within the spirit and principle of the present invention should be included in the protection scope of the present invention.

Claims (2)

1. A heterogeneous multi-robot positioning and control method based on an orchard map is characterized by comprising the following steps of:
(1) the piloting robot is remotely controlled by a person to walk in a fruit tree row, a complete orchard map is firstly constructed, and then through GNSS coordinate system conversion, all fruit tree center points in the orchard map are converted into GNSS absolute coordinate values [ tree ]i_gnss_x,treei_gnss_y](ii) a The navigation robot can estimate the nearest fruit tree coordinate [ tree ] in the range according to the radar scanning angle and the relative distancen_gnss_x,treen_gnss_y]By formula (1) and the coordinates [ tree ] of each fruit tree in the orchard mapi_gnss_x,treei_gnss_y]Comparison, when dminWhen the GNSS real coordinate is close to 0, the GNSS real coordinate of the fruit tree is obtained;
the real coordinate judgment calculation formula of the fruit tree closest to the robot is as follows:
Figure FDA0003178859400000011
(2) then according to the required fruit tree real coordinate1_gnss_x,tree1_gnss_y]、[tree2_gnss_x,tree2_gnss_y]、[tree4_gnss_x,tree4_gnss_y]Estimating reference navigation line equations of autonomous navigation of the piloting robot and the following robots 1 and 2 and ideal position coordinates [ X ] of the robots at the next moment by using row spacing r _ s and plant spacing p _ s1,Y1]，[X_f1,Y_f1]，[X_f2,Y_f2]；
The ideal position coordinate calculation formula of the robot is as follows:
Figure FDA0003178859400000012
Figure FDA0003178859400000013
Figure FDA0003178859400000014
wherein [ tree ]1_gnss_x,tree1_gnss_y]For piloting robot [270 degrees, 360 degrees ]]The nearest real coordinate of fruit tree 1, [ tree ]2_gnss_x,tree2_gnss_y]For piloting the robot [0 degrees, 90 degrees ]]The nearest fruit tree 2 true coordinate, [ tree ]4_gnss_x,tree4_gnss_y]For following the robot [270 degrees, 360 degrees ]]The nearest fruit tree 4 real coordinates.
(3) And calculating the transverse deviation and the course deviation from the ideal and actual position coordinates of each robot to the reference navigation line respectively, adjusting the advancing angular speed and linear speed of each robot in real time to enable each robot to reach the ideal position, and finishing the inter-row operation.
The robot reference leading line calculation formula is as follows:
Aj＝treei+1_gnss_y-treei_gnss_y
Bj＝treei_gnss_x-treei+1_gnss_x
Cj＝(treei+1_gnss_x-treei_gnss_y)-(treei_gnss_x-treei+1_gnss_y) (3)
A1Xi+B1Yi+C1＝0 (4)
A2Xi+B2Yi+C2＝0 (7)
wherein A isi Bi CiFor the coefficients of the robot reference course equation, equation (4) is the reference course 1 equation for the piloting robot and the following robot 1, and equation (7) is the reference for the following robot 2The reference lead line 2 equation for lead line 2.
2. The heterogeneous multi-robot positioning and control method based on the orchard map as claimed in claim 1, wherein the method for turning at the head of the orchard is characterized by comprising the following steps:
(1) obtaining an equation of a reference navigation line 1 according to the coordinates of the fruit trees 1 and 2 and the formulas (1), (2) and (5), and continuously generating an ideal position coordinate [ X ] of the navigation point 1 at the next moment through the formula (2) as the navigation robot advances1,Y1]Sending the coordinates of the following moment of the piloting robot to the following robot 1 and the following robot 2;
(2) the following robot 1 waits until the piloting robot runs to the 3 rd ideal position point, the following robot 1 reaches the 1 st point issued by the piloting robot, and the following robot stops moving after turning clockwise by 90 degrees;
(3) the following robot 2 waits for the following robot 1 to reach the 3 rd ideal position point issued by the piloting robot, the following robot 2 reaches the 2 nd point issued by the piloting robot, and the following robot stops moving after turning clockwise by 90 degrees;
(4) when the piloting robot reaches the 5 th ideal position coordinate, the following robot 1 starts to travel to the 4 th ideal position coordinate point and starts to turn clockwise by 90 degrees and then stops moving, and the following robot 2 can travel to the 3 rd ideal position coordinate point and starts to turn clockwise by 90 degrees and then stops moving; and when the three robots all reach the target point, the turning operation of the ground head is finished.
CN202110840772.6A 2021-07-25 2021-07-25 Heterogeneous multi-robot positioning and controlling method based on orchard map Active CN113485354B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110840772.6A CN113485354B (en) 2021-07-25 2021-07-25 Heterogeneous multi-robot positioning and controlling method based on orchard map

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110840772.6A CN113485354B (en) 2021-07-25 2021-07-25 Heterogeneous multi-robot positioning and controlling method based on orchard map

Publications (2)

Publication Number Publication Date
CN113485354A true CN113485354A (en) 2021-10-08
CN113485354B CN113485354B (en) 2023-07-04

Family

ID=77943470

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110840772.6A Active CN113485354B (en) 2021-07-25 2021-07-25 Heterogeneous multi-robot positioning and controlling method based on orchard map

Country Status (1)

Country Link
CN (1) CN113485354B (en)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116974291A (en) * 2023-09-25 2023-10-31 农业农村部南京农业机械化研究所 Control error determining method and device for master-slave cooperative navigation agricultural machinery

Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2020253316A1 (en) * 2019-06-18 2020-12-24 中国科学院上海微系统与信息技术研究所 Navigation and following system for mobile robot, and navigation and following control method
CN212683969U (en) * 2020-06-11 2021-03-12 西北农林科技大学 Orchard multi-robot physical model

Patent Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2020253316A1 (en) * 2019-06-18 2020-12-24 中国科学院上海微系统与信息技术研究所 Navigation and following system for mobile robot, and navigation and following control method
CN212683969U (en) * 2020-06-11 2021-03-12 西北农林科技大学 Orchard multi-robot physical model

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
孙弋;张雪丽;: "基于改进MSCKF算法的室内机器人定位方法", 计算机系统应用, no. 02 *

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116974291A (en) * 2023-09-25 2023-10-31 农业农村部南京农业机械化研究所 Control error determining method and device for master-slave cooperative navigation agricultural machinery

Also Published As

Publication number Publication date
CN113485354B (en) 2023-07-04

Similar Documents

Publication Publication Date Title
US20200278680A1 (en) Method and Device for Operating a Mobile System
CN106774310B (en) A kind of robot navigation method
CN110754204B (en) Lawn three-dimensional pattern trimming robot system and method
CN113359710B (en) LOS theory-based agricultural machinery path tracking method
CN110411421A (en) A kind of engineering construction automatic lofting system and automatic lofting method
CN108801269A (en) A kind of interior cloud Algorithms of Robots Navigation System and method
JP2022532126A (en) Mobility platform for autonomous navigation at construction sites
CN109002046A (en) A kind of Navigation System for Mobile Robot and air navigation aid
CN109917814A (en) Unmanned plane operational method and system
CN112882053A (en) Method for actively calibrating external parameters of laser radar and encoder
CN109753075B (en) Agriculture and forestry park robot navigation method based on vision
CN113485354A (en) Heterogeneous multi-robot positioning and control method based on orchard map
CN109407111A (en) A kind of tunnel three-dimensional scanner feature knowledge method for distinguishing
CN107567036B (en) SLAM system and method based on wireless self-organizing local area network of robot search and rescue environment
CN112309369A (en) Rice transplanter unmanned system and method based on voice recognition
CN106359012A (en) Circular and translation function integrated sprinkler and sprinkling method thereof
CN108646750B (en) Portable factory AGV following method based on UWB non-base station
Wu et al. Study of Autonomous Robotic Lawn Mower Using Multi-Sensor Fusion Based Simultaneous Localization and Mapping
CN106382916B (en) The double hexapod robot complicated landform 3 D stereo mapping systems and method of self-built referential
Hoang et al. Development of an EKF-based localization algorithm using compass sensor and LRF
CN215494708U (en) Intelligent terrace marking robot
Avanzini et al. A control strategy taking advantage of inter-vehicle communication for platooning navigation in urban environment
CN114371699B (en) Portable small base station group type agricultural machinery UWB navigation system and method for small and medium farmland
CN104908030A (en) SCARA robot accurate positioning method based on granularity model
CN113568432B (en) Unmanned aerial vehicle accurate landing method and system based on cloud computing and image recognition detection

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