CN-121995923-A - Robot path tracking method, medium and equipment

CN121995923ACN 121995923 ACN121995923 ACN 121995923ACN-121995923-A

Abstract

A robot path tracking method, medium and equipment belong to the technical field of robot motion control. The method aims at solving the technical problem that the navigation controller cannot acquire correct steering direction because the coupling association of the robot position and the course angle is not constructed in the existing robot path tracking method. The method comprises the steps of obtaining initial transverse deviation based on an actual course angle of a robot and a current operation path, giving positive and negative signs for the initial transverse deviation by utilizing a robot operation path point sequence, a real-time position and a real-time course angle, taking the positive and negative signs as the transverse deviation, and obtaining course deviation which enables a controller to select a minimum rotation angle based on a front view point of an agricultural machine, the actual course angle and the operation path point sequence. And (3) weighting the course deviation and the transverse deviation to obtain a composite error, and outputting a correct steering instruction by the navigation controller according to a corrected corner required by eliminating the composite error to realize the path tracking of the robot. The robot path tracking method is mainly used for robot path tracking.

Inventors

ZHOU BAOMIN
WEN LIANG
LI TIELEI
CHU CHAO
ZHOU YICHEN
REN RONGRONG
Wang Junchuo
FENG GUOHUI
Zhou Geming
ZHANG YANG

Assignees

北大荒信息有限公司
哈尔滨工程大学

Dates

Publication Date: 20260508
Application Date: 20260212

Claims (10)

1. A robot path tracking method, comprising: Step S1, acquiring a working path point sequence and a current position of a robot in real time And heading angle ; Step S2, based on the operation path point sequence and the current position of the robot Obtaining a first path point First subsequent point And directed area And then based on the first path point First subsequent point Obtaining unsigned initial lateral deviation Reusing the first path point Pointing to a first successor point Vector of (3) Included angle of axial half shaft And robot heading angle And directed area Determining unsigned initial lateral deviation To give the sign an initial lateral deviation I.e. lateral deviation ; Step S3, based on the operation path point sequence and the current position of the robot And heading angle Obtaining a second path point Second subsequent point Based on the second path point Second subsequent point And heading angle Obtaining course deviation ; Step S4, based on the lateral deviation And heading deviation Obtaining a synthetic error And then based on the synthesis error Obtaining the rotation angle The motion controller corrects the rotation angle And the control signals are converted into motor control signals to drive the robot to turn to the motor, so that path tracking is realized.
2. The method of claim 1, wherein the sequence of points of the robot's working path is selected from the sequence of points of the robot's working path and the current position of the robot's working path The path point with the smallest Euclidean distance and the subsequent point of the path point are taken as the first path point First subsequent point 。
3. A robot path tracking method according to claim 2, characterized in that the directional area The method comprises the following steps: Based on the first path point Pointing to a first successor point Is the path tangential vector of (2) First path point Pointing to the current position of the robot Is of the deviation vector of (2) Determining the directed area 。
4. A method of robot path tracking according to claim 3, characterized in that the unsigned initial lateral deviation Wherein And As an intermediate variable, the number of the variables, , 。
5. A robot path tracking method according to claim 4, characterized in that the unsigned initial lateral deviation The positive and negative sign judgment rules of (1) include: Rule one is when the included angle Course angle with robot Satisfy the following requirements And directed area At the time of initial lateral deviation Giving a negative sign; rule II, when the included angle is Course angle with robot Satisfy the following requirements And directed area At the time of initial lateral deviation Giving a positive sign; Rule III, when included angle Course angle with robot Satisfy the following requirements And directed area At the time of initial lateral deviation Giving a negative sign; Rule IV, when included angle Course angle with robot Satisfy the following requirements And directed area At the time of initial lateral deviation Giving a positive sign; Rule V when the included angle Course angle with robot Satisfy the following requirements And directed area At the time of initial lateral deviation Giving a negative sign; Rule six, when the included angle Course angle with robot Satisfy the following requirements And directed area At the time of initial lateral deviation Giving a positive sign; Rule seven, when the included angle Course angle with robot Satisfy the following requirements And directed area At the time of initial lateral deviation Giving a positive sign; rule eight, when included angle Course angle with robot Satisfy the following requirements And directed area At the time of initial lateral deviation A value of 0; Rule nine when the included angle Course angle with robot Satisfy the following requirements And directed area At the time of initial lateral deviation Giving a negative sign; rule ten, when included angle Course angle with robot Satisfy the following requirements 、 And directed area At the time of initial lateral deviation Giving a positive sign; Rule eleven, when included angle Course angle with robot Satisfy the following requirements 、 And directed area At the time of initial lateral deviation Giving a negative sign; Rule twelve, when included angle Course angle with robot Satisfy the following requirements 、 And directed area At the time of initial lateral deviation Giving a positive sign; Rule thirteen when the included angle Course angle with robot Satisfy the following requirements 、 And directed area At the time of initial lateral deviation Giving a negative sign; Rule fourteen when the included angle Course angle with robot Satisfy the following requirements 、 And directed area At the time of initial lateral deviation Giving a positive sign; Rule fifteen when the included angle Course angle with robot Satisfy the following requirements 、 And directed area At the time of initial lateral deviation A negative sign is given.
6. The method of claim 5, wherein step S3 includes: step S31, based on the current position of the robot And heading angle Determining a front viewpoint ; Step S32, selecting the front view point and the operation path point sequence of the robot A path point with the smallest Euclidean distance and a subsequent point of the path point are taken as second path points Second subsequent point ; Step S33, based on the second path point Pointing to a second successor point Vector of (3) Included angle of axial half shaft And robot heading angle Obtaining course deviation 。
7. The method of claim 6, wherein step S33 obtains a heading deviation The process of (1) comprises: rule A when included angle Course angle And is also provided with When (1): Heading deviation The calculation mode of (a) is as follows: ; Rule B when the included angle Course angle And is also provided with When (1): Heading deviation The calculation mode of (a) is as follows: ; rule C, when included angle And course angle When (1): Heading deviation The calculation mode of (a) is as follows: ; Rule D, when included angle Course angle And is also provided with When (1): Heading deviation The calculation mode of (a) is as follows: ; Rule E, when included angle Course angle And is also provided with When (1): Heading deviation The calculation mode of (a) is as follows: ; Rule F, when included angle And course angle When (1): Heading deviation The calculation mode of (a) is as follows: 。
8. the method of claim 7, wherein the error in step S4 is , 、 Respectively represent the transverse deviation Weight coefficient and heading deviation of (2) Weight coefficient of (c) in the above-mentioned formula (c).
9. Computer storage medium, characterized in that it has stored therein at least one instruction that is loaded and executed by a processor to implement a robot path tracking method according to any of claims 1 to 8.
10. A robot path tracking method generating apparatus, characterized in that the apparatus comprises a processor and a memory, the memory having stored therein at least one instruction which is loaded and executed by the processor to implement a robot path tracking method according to any one of claims 1 to 8.

Description

Robot path tracking method, medium and equipment Technical Field The invention relates to the technical field of robot motion control, in particular to a robot path tracking method, a medium and equipment. Background ‌ With the rapid development and wide application of accurate agricultural technology, real-time dynamic positioning technology (Real-TIME KINEMATIC ‌ -Global Navigation SATELLITE SYSTEM, RTK-GNSS) based on Beidou satellite navigation system (BDS) has become the core standard of modern agricultural machinery equipment by virtue of positioning advantages of high precision and high reliability. In a field actual operation scene, the agricultural machinery often needs to complete reciprocating type farming, seeding, fertilizing and other operation tasks according to preset paths such as a back character shape, a bow character shape and the like, and strict requirements are provided for automatic navigation and path tracking precision of the agricultural robot. The existing path tracking method generally takes a geometric projection principle as a core to calculate navigation deviation, wherein the navigation deviation comprises transverse deviation and heading deviation. The lateral deviation is defined as the minimum euclidean distance of the real-time position of the robot to the preset path, the sign of which is determined only by the geometric sideways direction determined by the vector cross product. The calculation mode only reflects the static geometric position of the robot body and does not integrate the key course attitude information. And the course deviation is calculated by presetting a forward looking distance, selecting a corresponding forward viewpoint and then solving the difference between the current course angle of the robot and the tangential angle of the path at the forward viewpoint. The path tracking method only depends on the geometric positions of the agricultural robot and preset path points to determine navigation deviation and deviation signs, lacks coupling analysis on the position and the course angle of the robot, ignores the relative situation of the real-time course and the path trend of the robot, and fails to establish the coupling relation between the deviation signs and the direction of course angle adjustment, so that a navigation controller cannot acquire correct steering guidance under complex operation paths such as a back character pattern, a bow character pattern and the like. Disclosure of Invention The application provides a robot path tracking method, which aims to solve the technical problem that the navigation controller cannot acquire correct steering directions because the coupling association of the robot position and the course angle is not constructed in the traditional robot path tracking method. Furthermore, the application also provides a corresponding computer readable storage medium and equipment. The first aspect of the present invention provides a robot path tracking method, including: Step S1, acquiring a working path point sequence and a current position of a robot in real time And heading angle; Step S2, based on the operation path point sequence and the current position of the robotObtaining a first path pointFirst subsequent pointAnd directed areaAnd then based on the first path pointFirst subsequent pointObtaining unsigned initial lateral deviationReusing the first path pointPointing to a first successor pointVector of (3)Included angle of axial half shaftAnd robot heading angleAnd directed areaDetermining unsigned initial lateral deviationTo give the sign an initial lateral deviationI.e. lateral deviation; Step S3, based on the operation path point sequence and the current position of the robotAnd heading angleObtaining a second path pointSecond subsequent pointBased on the second path pointSecond subsequent pointAnd heading angleObtaining course deviation; Step S4, based on the lateral deviationAnd heading deviationObtaining a synthetic errorAnd then based on the synthesis errorObtaining the rotation angleThe motion controller corrects the rotation angleAnd the control signals are converted into motor control signals to drive the robot to turn to the motor, so that path tracking is realized. Further, selecting the current position and the current position in the operation path point sequence of the robotThe path point with the smallest Euclidean distance and the subsequent point of the path point are taken as the first path pointFirst subsequent point。 Further, the directional areaThe method comprises the following steps: Based on the first path point Pointing to a first successor pointIs the path tangential vector of (2)First path pointPointing to the current position of the robotIs of the deviation vector of (2)Determining the directed area。 Further, the unsigned initial lateral deviationWhereinAndAs an intermediate variable, the number of the variables,,。 Further, the unsigned initial lateral deviationThe positive and negative