CN117864167A - Magnetic nail positioning arrangement method suitable for double magnetic sensing and AGV tracking control method - Google Patents
Magnetic nail positioning arrangement method suitable for double magnetic sensing and AGV tracking control method Download PDFInfo
- Publication number
- CN117864167A CN117864167A CN202410019030.0A CN202410019030A CN117864167A CN 117864167 A CN117864167 A CN 117864167A CN 202410019030 A CN202410019030 A CN 202410019030A CN 117864167 A CN117864167 A CN 117864167A
- Authority
- CN
- China
- Prior art keywords
- straight line
- front wheel
- magnetic
- mode
- error
- 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
- 238000000034 method Methods 0.000 title claims abstract description 58
- 238000004364 calculation method Methods 0.000 claims description 21
- 230000003068 static effect Effects 0.000 claims description 9
- 238000012937 correction Methods 0.000 claims description 7
- 230000001186 cumulative effect Effects 0.000 claims description 6
- 230000006698 induction Effects 0.000 claims description 6
- 238000005259 measurement Methods 0.000 claims description 4
- 238000005516 engineering process Methods 0.000 abstract description 7
- 238000011065 in-situ storage Methods 0.000 description 8
- 238000010586 diagram Methods 0.000 description 6
- 230000000694 effects Effects 0.000 description 3
- 230000009286 beneficial effect Effects 0.000 description 2
- 230000009977 dual effect Effects 0.000 description 2
- 230000007613 environmental effect Effects 0.000 description 2
- 238000012545 processing Methods 0.000 description 2
- 101100537937 Caenorhabditis elegans arc-1 gene Proteins 0.000 description 1
- 238000010276 construction Methods 0.000 description 1
- 238000007689 inspection Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
Landscapes
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The invention relates to the field of component positioning arrangement methods in magnetic sensing technology and the technical field of automatic driving tracking control, in particular to a magnetic nail positioning arrangement method suitable for double magnetic sensing.
Description
Technical Field
The invention relates to the field of component positioning and arranging methods in magnetic sensing technology and the technical field of automatic driving tracking control.
Background
The magnetic nail navigation is a navigation system based on the magnetic nail technology, and can provide accurate positioning and navigation functions. The magnetic nails are used as small magnetic objects placed on the ground, unique magnetic field signals can be generated, the magnetic nail navigation system can realize the positioning and navigation of the vehicle by utilizing the magnetic field signals, the magnetic nail navigation system needs to develop corresponding positioning and navigation algorithms, calculates an optimal navigation path according to the current position and the target position of the vehicle, provides real-time navigation guidance, and helps the vehicle to accurately reach the destination. The magnetic nail navigation technology realizes the accurate positioning and navigation functions in the indoor environment by utilizing the technologies of a magnetic field sensor, a positioning algorithm, map construction and calibration, a navigation algorithm and the like, and has wide application prospects in the fields of indoor navigation, unmanned vehicles, intelligent robots and the like. The magnetic nails are guided according to the different selected magnetic nails and the different arrangement modes of the magnetic field sensors, and the corresponding positioning and guiding modes are also different. The magnetic nails with ID information can provide higher positioning precision, but common magnetic nails can only provide relatively simple positioning functions, and have wide application scenes due to lower price, but have stricter requirements on the arrangement of the magnetic nails, and the double-magnetic-field sensor can provide heading information, but the arrangement of magnetic nail routes of vehicles which cannot be steered in situ is more complicated.
The existing magnetic nail positioning scheme is mainly aimed at a chassis system capable of rotating in situ, but in a transport vehicle for automatic driving logistics transportation, an automatic inspection operation robot and the like, a plurality of intelligent transport vehicles and chassis systems of the robots are incapable of rotating in situ, so that the application of the existing magnetic nail positioning scheme has a large limitation.
In addition, the tracking control technique of the autopilot refers to a technique for realizing vehicle tracking and control in an autopilot system. It is an important component in an autopilot system to ensure that the vehicle can accurately track a selected path and control based on real-time environment and sensor data to achieve safe, stable and efficient travel. Tracking control techniques use control algorithms to calculate control commands for the vehicle to achieve a desired trajectory tracking, common control algorithms include PID control, model Predictive Control (MPC), and the like. Model predictive control predicts vehicle behavior over a period of time in the future by building a vehicle dynamics model and an environmental model, and controls based on the prediction results to achieve a more accurate tracking effect. Tracking control techniques require control and adjustment in real time based on vehicle conditions and environmental changes. By collecting and processing the sensor data in real time, the tracking control system can continuously update the state and environment information of the vehicle and adjust the control instruction according to the feedback information so as to realize an accurate tracking effect. In general, the performance of the algorithm can be exerted to the greatest extent only by using a corresponding tracking control algorithm according to a specific scene and a vehicle, so that the optimal control effect is realized.
The existing tracking control technology is mainly oriented to a differential model chassis, has few control modes aiming at a chassis model with front and rear wheels steering independently and being unable to rotate in situ, and the existing PID tracking control method has larger tracking error for a turning scene. Aiming at the limitation, the invention provides a realization method for tracking control on a path consisting of a straight line and an arc aiming at a chassis model with independent steering of front wheels and rear wheels and non-in-situ rotation.
Disclosure of Invention
The invention aims to provide a magnetic nail positioning arrangement method suitable for double magnetic sensing, which is suitable for a chassis system with four wheels capable of steering independently and not rotating in situ.
The invention further aims to provide an AGV tracking control method for sensing, positioning and tracking control by combining the magnetic nail positioning arrangement method suitable for double magnetic sensing, and provides a method for realizing tracking control on a path formed by a straight line and an arc by aiming at a chassis model which is independently steered and can not rotate in situ for front wheels and rear wheels aiming at the limitation of the background technology.
In order to achieve the above purpose, the technical scheme of the invention is as follows: the chassis system of the vehicle adopting the magnetic nail positioning arrangement mode is a chassis system with front wheels and rear wheels capable of being independently controlled in a steering way, and the center of the front axle and the center of the rear axle of the vehicle are respectively provided with magnetic sensors, and the magnetic nail positioning arrangement mode comprises a magnetic nail arrangement flow, and the specific magnetic nail arrangement flow is as follows:
1) The method comprises the steps that a center point of a front axle and a rear axle of a vehicle is taken as a datum point, a magnetic nail arrangement route is determined according to a movement track of the datum point, the magnetic nail arrangement route only comprises a straight line and various arcs with fixed radiuses, an arc junction section and a straight line junction section are formed at the junction of the arcs and the straight line in the magnetic nail arrangement route, and two ends of the arc junction section and the straight line junction section are intersection points of the straight line and the arcs;
2) Determining the wheelbase between the front axle and the rear axle, taking the wheelbase as a magnetic nail induction interval distance d, and performing integer division on the magnetic nail induction interval distance d to determine a magnetic nail arrangement interval distance s;
3) Sequentially placing magnetic nails in full segments at equal intervals on a straight line or an arc according to an arrangement interval distance s from a first placement starting point by taking a certain position of the non-straight line junction section of one straight line or a certain position of the non-arc junction section of one arc of the magnetic nail arrangement routes as the first placement starting point;
4) Ensuring that the magnetic nails are placed at two intersection points of the straight line (or the circular arc) and the circular arc (or the straight line);
5) The magnetic nails are placed continuously in the straight line or placed continuously in the straight line, the interval point of the continuous straight line or the straight line from the first intersection point of the straight line or the straight line before connection is a second placement starting point, the magnetic nails are sequentially placed on the continuous straight line or the straight line according to the placement interval distance s from the second placement starting point, the intersection point interval distance ss is larger or smaller than the placement interval distance s, and the magnetic nails on the two sections of the intersection section and the straight line intersection section are ensured to be staggered.
The AGV tracking control method on the path of the magnetic nails is arranged by the magnetic nail positioning arrangement method suitable for double magnetic sensing, the magnetic nails on the path are respectively sensed by two magnetic sensors on an AGV vehicle, the tracking control method comprises the following steps that the tracking control of a straight line path is marked as a straight line mode and the tracking control of a turning path is marked as a turning mode, the tracking control of the straight line mode and the turning mode is carried out and comprises the tracking control of a front wheel and a rear wheel, the front wheel and the rear wheel can be respectively and independently controlled in a steering mode, the tracking control method is described by taking the tracking control of the front wheel as an example, the steps of the executing method of the rear wheel can be obtained by referring to the front wheel equally, and the steps of the front wheel tracking control method of the straight line mode and the turning mode are as follows:
straight line mode:
1) Obtaining magnetic navigation positioning;
2) The given linear velocity is a fixed value;
3) Calculating to obtain a transverse deviation error, an accumulated transverse deviation error and a heading error of a front axle center of the front wheel to an arc path;
4) Determining a PID coefficient, and calculating a dynamic front wheel deflection angle value corresponding to the PID;
5) Determining a final front wheel deflection angle value;
6) The final front wheel deflection angle w obtained through calculation is used as an actual control value to be output to a controller, and the front wheel is controlled to deflect according to a set value;
7) Returning to step 2), the calculation is performed in a loop. Thus realizing PID tracking control on the front wheels of the vehicle;
turning mode:
s1) obtaining magnetic navigation positioning;
s2) the given linear velocity is a fixed value;
s3) calculating to obtain a transverse offset error, an accumulated transverse offset error and a heading error of a front axle center of the front wheel to an arc path;
s4) determining PID coefficients, and calculating dynamic front wheel deflection angle values corresponding to the PID;
s5) determining a static deflection angle value corresponding to the front tire;
s6) determining a final front wheel deflection angle value;
s7) outputting the final front wheel deflection angle w obtained through calculation to a controller as an actual control value, and controlling the front wheel to deflect according to a set value;
s8) returning to the step 2), and circularly performing calculation.
In the straight line mode:
step 3) is as follows:
through steps 1) and 2), first, calculate the lateral offset error of the front axle center of the front wheel to the circular arc path and record as d ef And the lateral error is set to be positive when the front wheel is outside the path and negative when the front wheel is inside the path;
next, the accumulated lateral offset error is calculated and noted as i ef The accumulated lateral offset error is the accumulated sum of the lateral offset errors calculated from the start time to the current time;
then, calculate heading error and mark as θ ef The course error is recorded as yaw by taking anticlockwise as positive and the course error as the front wheel course f The reference course is recorded as yaw rf The difference of (a), i.e. θ ef ＝yaw f -yaw rf The reference course is the corresponding tangential direction of the nearest point of the front wheel on the circular arc;
step 4) is to calculate the lateral error d in step 3) ef Cumulative lateral error i ef And heading error θ ef Then, determining PID parameters through debugging, and obtaining a corresponding dynamic front wheel deflection angle w 1 ＝Pd ef +Ii ef +Dθ ef 。
Step 5) is such that the final front wheel yaw angle for control is denoted as w, and the dynamic front wheel yaw angle w obtained in step 4) is 1 ＝Pd ef +Ii ef +Dθ ef As the front wheel yaw value finally used for control, i.e. w=w 1 ；
And/or, in the turning mode:
step S3) is as follows:
through steps S1) and S2), first, calculate the lateral offset error of the front axle center of the front wheel to the circular arc path and record as d ef And the lateral error is set to be positive when the front wheel is outside the path and negative when the front wheel is inside the path;
next, the accumulated lateral offset error is calculated and noted as i ef The accumulated lateral offset error is the accumulated sum of the lateral offset errors calculated from the start time to the current time;
then, calculate heading error and mark as θ ef The course error is recorded as yaw by taking anticlockwise as positive and the course error as the front wheel course f The reference course is recorded as yaw rf The difference of (a), i.e. θ ef ＝yaw f -yaw rf The reference course is the corresponding tangential direction of the nearest point of the front wheel on the circular arc;
step S4) is to calculate the lateral error d in step S3) ef Cumulative lateral error i ef And heading error θ ef Then, determining PID parameters through debugging, and obtaining a corresponding dynamic front wheel deflection angle marked as w 1 ，
w 1 ＝Pd ef +Ii ef +Dθ ef ；
Step S5) is recorded as d according to the wheelbase w The static deflection angle value corresponding to the front wheel tire is determined to be w 2 ，
Step 6) calculating the final front wheel deflection angle for control as w=w according to the dynamic front wheel deflection angle and the static front wheel deflection angle 1 +w 2 。
Step 1) and step 1) are to obtain the magnetic navigation positioning of two magnetic sensors; the step 3) and the step 3) are performed in such a manner that when the magnetic sensors sense the magnetic nails at the same time, the actual value of the transverse error is directly sensed by the magnetic sensors to be used as a correction value to replace the transverse offset error for calculation, and when the magnetic sensors do not sense the magnetic nails at the same time, the sensor data of the odometer or the inertial measurement unit and the like are acquired to calculate the transverse offset error based on the latest correction value.
The AGV vehicle is controlled by selecting different modes at different road sections, selecting straight line mode control at a straight line road section, selecting turning mode control at an arc road section, and performing automatic switching mode at the joint point of the straight line road section and the arc road section, wherein the switching mode is as follows: taking the center of a connecting line of a front wheel and a rear wheel shaft of the vehicle as a reference point, connecting a running track of the reference point by a straight line and an arc, taking a straight line section as an example, starting from the straight line section, and controlling in a straight line mode; when the datum point moves to the joint point of the linear track and the circular arc track, switching from the linear mode to the turning mode for control, and moving along the circular arc; when the datum point continues to run to the joint point of the circular arc and the linear track, the control is performed by switching from the turning mode to the linear mode.
By adopting the technical scheme, the invention has the beneficial effects that: the invention relates to a magnetic nail positioning mode suitable for front and rear double magnetic sensors, which is suitable for a front and rear axle independent steering chassis control vehicle, and is characterized in that according to a front and rear magnetic sensor, a magnetic nail route is divided into two combined arrangement modes of a straight line and a fixed radius arc, the straight line route and the arc route are combined into a whole AGV movement route, the straight line is arranged by equally-spaced straight line magnetic nails, the pure arc is arranged by equally-spaced arc, a mixing section is arranged in a dislocation mode, one sensor senses magnetic nails at the other position on a mixing section, and the sensing mode of magnetic nail positioning information can be simultaneously sensed by two sensors to be regarded as effective data, so that the magnetic sensors can be prevented from reading and processing error magnetic nail data. The invention is beneficial to the realization of the pure magnetic nail positioning mode of the AGV in the factory logistics.
The AGV tracking control method is based on the magnetic nail positioning arrangement method suitable for double magnetic sensing, is mainly suitable for independent steering control of a front wheel front axle and a rear wheel rear axle, and a chassis system is an AGV vehicle which can not steer in situ, and performs tracking control on a straight line and an arc path.
Drawings
Fig. 1 is a schematic diagram of a line-to-arc path in a magnetic nail positioning method suitable for front and rear magnetic sensors according to the present invention.
Fig. 2 is a schematic diagram of a circular arc-to-straight line path in a magnetic nail positioning mode suitable for front and rear magnetic sensors according to the present invention.
Fig. 3 is a schematic diagram of a path of a straight line in two different directions in a magnetic nail positioning mode suitable for front and rear magnetic sensors according to the present invention.
Fig. 4 is a schematic diagram of a path of a straight line extending straight line and dividing into an arc in a magnetic nail positioning mode suitable for front and rear magnetic sensors according to the present invention.
Fig. 5 is a schematic diagram of a line with multiple combinations of straight arcs in a magnetic nail positioning mode suitable for front and rear magnetic sensors according to the present invention.
Fig. 6 and 7 are schematic diagrams of three coordinate systems of a vehicle in different modes in a dual PID tracking control method suitable for front and rear axle independent steering AGVs according to the present invention.
FIG. 8 is a schematic view of the engagement points of the switching mode in a dual PID tracking control method for an independent front and rear axle steering AGV in accordance with the present invention.
Detailed Description
In order to further explain the technical scheme of the invention, the invention is explained in detail by specific examples.
The chassis system of the vehicle adopting the magnetic nail positioning arrangement mode disclosed by the embodiment of the invention is a chassis system with front wheels and rear wheels capable of being independently controlled in steering, and the center of the front axle and the center of the rear axle of the vehicle are respectively provided with magnetic sensors, namely the vehicle is provided with double magnetic sensors, the magnetic nail positioning arrangement mode comprises a magnetic nail arrangement flow, and the specific magnetic nail arrangement flow is as follows.
1) The method comprises the steps of determining a magnetic nail arrangement route by taking the center points of a front axle and a rear axle of a vehicle as reference points according to the movement track of the reference points, wherein the magnetic nail arrangement route is only composed of straight lines and arcs with various fixed radiuses, the turning radius of each arc with various fixed radiuses, namely the turning radius at each turning road section, is a fixed value, and according to the sensing of a double magnetic sensor for vehicle running, a mixed road section with an arc junction section and a straight line junction section formed at the junction of the arc and the straight line in the magnetic nail arrangement route is carried out, and two ends of the arc junction section and the straight line junction section are the intersection point of the straight line and the arc, namely the intersection point 1 and the intersection point 2 shown in the figures.
2) And determining the wheelbase between the front axle and the rear axle, taking the wheelbase as a magnetic nail induction interval distance d, and performing integer division on the magnetic nail induction interval distance d to determine the arrangement interval distance s of the magnetic nails.
3) The method comprises the steps of sequentially placing magnetic nails in a straight line or an arc according to an arrangement interval distance s at equal intervals by taking a certain position of a non-straight line junction section of one straight line of a magnetic nail arrangement route or a certain position of a non-arc junction section of one arc as a first placement starting point.
4) Ensuring that the magnetic nails are placed at two intersection points of the straight line (or the circular arc) and the circular arc (or the straight line).
5) The magnetic nails are placed continuously in the straight line or placed continuously in the straight line, the interval point of the continuous straight line or the straight line from the first intersection point of the straight line or the straight line before connection is a second placement starting point, the magnetic nails are sequentially placed on the continuous straight line or the straight line according to the placement interval distance s from the second placement starting point, the intersection point interval distance ss is larger or smaller than the placement interval distance s, and the magnetic nails on the two sections of the intersection section and the straight line intersection section are ensured to be staggered.
If the magnetic nails are to be arranged in a continuous straight line or arc, the step 5) is repeated for placing the magnetic nails until the arrangement is completed.
The magnetic nails are positioned and arranged in a program processing mode, the program is processed by the front magnetic sensor and the rear magnetic sensor along the magnetic nails arranging route along the moving direction of the vehicle, and the program only reads the magnetic nails data at the same time by the two magnetic sensors, so that the data can be regarded as effective data to be processed.
The above method can be applied to several routes of fig. 1-5 and to the magnetic nail positioning arrangement of a combination of these routes.
As shown in fig. 1, the magnetic nails are arranged in a straight line to circular arc path, and the magnetic nails are arranged at equal intervals from the start point of the straight line in the figure for the initial straight line path by an arrangement interval distance s until just to the last intersection point of the straight line and the circular arc, and the intersection point 2 in the figure is assumed that the start point starts from the straight line.
After the arrangement of the straight line and the intersection point is finished, the arrangement of the magnetic nails on the circular arc is started, if the equal interval arrangement is directly started from the intersection point 1 magnetic nails in the graph, the magnetic nails of the circular arc and the straight line magnetic nails are excessively close to each other in a mixed road section, the magnetic nails can be simultaneously induced in operation, and the calculation of the positioning of the magnetic nails can be interfered, so that the circular arc cannot take the position corresponding to the intersection point 1 in the graph as an arrangement starting point of the circular arc, but takes the intersection point interval distance ss set from the intersection point magnetic nails as a new starting point, and the intersection point interval distance ss is smaller than or larger than the arrangement interval distance s so as to ensure that the magnetic nails of the circular arc and the magnetic nails of the straight line are completely staggered.
As shown in fig. 2, in the magnetic nails arrangement mode of the circular arc to straight line path, assuming that the starting point starts somewhere in the circular arc, the magnetic nails are arranged at equal intervals s from the starting point for the circular arc path until just to the last intersection point of the circular arc and the straight line, and the intersection point 2 in fig. 2.
After the circular arc is arranged, the arrangement of the magnetic nails on the straight line is started. Likewise, if the magnetic nails are arranged at equal intervals directly from the intersection point 1 in fig. 2, the mixing sections of the straight magnetic nails and the circular arc magnetic nails are too close, and the magnetic nails are positioned to be interfered. Therefore, the straight line cannot take the intersection point as the initial magnetic nail, the intersection point interval distance ss which is set from the intersection point magnetic nail distance is taken as a new starting point, ss is smaller or larger than s, then the magnetic nails are arranged at equal intervals s from the initial magnetic nail continuously from the new starting point, so that the complete staggering of the circular arc magnetic nails and the straight line path is ensured.
Some road segments may have three routes converging together. As shown in fig. 3, a situation that one straight line section is divided into two arcs (arc 1 and arc 2 in the drawing) is equivalent to superposition of two straight lines and arcs, in order to simplify the arrangement of the magnetic nails, the intersection points of the two straight lines and the arc sections are set to be the same place, and then the arrangement mode of the magnetic nails is equivalent to superposition of the straight line-left turning arc and the straight line-right turning arc, namely, the arrangement mode of the arc on the other side is added after the arrangement mode of fig. 1.
In fig. 4, a straight line section is shown to be a case where an arc of a circle and a straight line are separated somewhere, which is simpler, and only the straight lines are required to be continuously distributed at equal intervals, and then the arrangement of the magnetic nails on the arc of fig. 1 is started.
By the above specific implementation manners, it can be known that the above specific implementation manners can form any path, as shown in fig. 5, the whole path is composed of a straight line and an arc, that is, a straight line-arc-straight line path, that is, the arrangement in the manner of fig. 2 is added after the manner of fig. 1.
By the AGV tracking control method on the paths where the magnetic nails are arranged by the magnetic nail positioning arrangement method suitable for double magnetic sensing, a front axle of an AGV vehicle which is a front wheel and a rear axle of a rear wheel can be respectively and independently controlled in steering, and the tracking control method comprises the following control of a straight path is recorded as a straight mode and the following control of a turning path is recorded as a turning mode. Since the present invention is directed to an AGV vehicle in which a front axle and a rear axle can be independently steered, front wheels and rear wheels of the vehicle can be independently steered, respectively, and in a tracking control method for the rear wheels, whether in a straight mode or a cornering mode, the method steps performed by the method can be equally obtained with reference to the front wheels, and the following description will mainly be made with reference to the front wheels as an example.
The linear mode is a common PID control mode, and the tracking control method comprises the following steps:
1) Obtaining the magnetic navigation positioning.
The tracking control mainly focuses on three coordinate systems, namely a vehicle front wheel coordinate system, a vehicle rear wheel coordinate system and a vehicle center coordinate system, and as shown in fig. 6, the positioning of the vehicle center relative to the magnetic navigation coordinate system is obtained through the relation among the coordinate systems, so that the positioning of the vehicle center in the magnetic navigation coordinate system is obtained.
2) The given linear velocity is a fixed value.
The linear speeds of the front and rear wheels of the vehicle are uniform, giving a linear speed value.
3) And calculating to obtain the transverse offset error, the accumulated transverse offset error and the heading error of the front axle center of the front wheel to the circular arc path.
Through steps 1) and 2), first, calculate the lateral offset error of the front axle center of the front wheel to the circular arc path and record as d ef As shown in FIG. 6Namely the shortest distance between the front wheel and the path, and the transverse error is set to be positive when the front wheel is outside the path and negative when the front wheel is inside the path;
next, the accumulated lateral offset error is calculated and noted as i ef The accumulated lateral offset error is the accumulated sum of the lateral offset errors calculated from the start time to the current time;
then, calculate heading error and mark as θ ef As shown in FIG. 6, the heading error is the heading of the front wheel and is recorded as yaw f The reference course is recorded as yaw rf The difference of (a), i.e. θ ef ＝yaw f -yaw rf Wherein the reference heading is the corresponding tangential direction of the nearest point of the front wheel on the circular arc, as indicated by the arrow direction in fig. 6.
4) And determining a PID coefficient, and calculating a dynamic front wheel deflection angle value corresponding to the PID.
In step 3) the lateral error d is calculated ef Cumulative lateral error i ef And heading error θ ef Then, determining PID parameters through debugging, and obtaining a corresponding dynamic front wheel deflection angle w 1 ＝Pd ef +Ii ef +Dθ ef 。
5) A final front wheel yaw angle value is determined.
The final front wheel yaw angle for control is denoted as w, and the dynamic front wheel yaw angle w obtained by step 4) is calculated as w 1 ＝Pd ef +Ii ef +Dθ ef As the front wheel yaw value finally used for control, i.e. w=w 1 . The calculation of the yaw angle of the rear wheels is identical to the calculation of the yaw angle of the front wheels.
6) And outputting the final front wheel deflection angle w obtained through calculation to a controller as an actual control value, and controlling the front wheels to deflect according to a set value.
7) Returning to step 2), the calculation is performed in a loop. Thus realizing PID tracking control on the front wheels of the vehicle.
The steps of the method for PID tracking control of the rear wheels of the vehicle are consistent with those of the method for PID tracking control of the front wheels, and can be directly substituted and deduced by a person skilled in the art through the content, so that the description is not repeated in the embodiment, and the clear understanding of the technical scheme of the invention by the person skilled in the art is not affected.
The PID control mode of static value plus dynamic adjustment is adopted in the turning mode with fixed turning radius, and the tracking control method comprises the following steps:
s1) obtaining the magnetic navigation positioning.
The tracking control mainly focuses on three coordinate systems, namely a vehicle front wheel coordinate system, a vehicle rear wheel coordinate system and a vehicle center coordinate system, and as shown in fig. 7, the positioning of the vehicle center relative to the magnetic navigation coordinate system is obtained through the relation among the coordinate systems, so that the positioning of the vehicle center in the magnetic navigation coordinate system is obtained.
S2) the given linear velocity is a fixed value.
The linear speeds of the front and rear wheels of the vehicle are uniform, giving a linear speed value.
S3) calculating to obtain the transverse offset error, the accumulated transverse offset error and the heading error of the front axle center of the front wheel to the circular arc path.
Through steps S1) and S2), first, calculate the lateral offset error of the front axle center of the front wheel to the circular arc path and record as d ef As shown in fig. 7, i.e., the shortest distance between the front wheel and the path, the lateral error is set to be positive when the front wheel is outside the path, and the lateral error is set to be negative when the front wheel is inside the path;
next, the accumulated lateral offset error is calculated and noted as i ef The accumulated lateral offset error is the accumulated sum of the lateral offset errors calculated from the start time to the current time;
then, calculate heading error and mark as θ ef As shown in FIG. 7, the heading error is the heading of the front wheel and is recorded as yaw f The reference course is recorded as yaw rf The difference of (a), i.e. θ ef ＝yaw f -yaw rf Wherein the reference heading is the corresponding tangential direction of the nearest point of the front wheel on the circular arc, as indicated by the arrow direction in fig. 7.
S4) determining PID coefficients, and calculating dynamic front wheel deflection angle values corresponding to the PID.
In step S3) a lateral error d is calculated ef Cumulative lateral error i ef And heading error θ ef Then, determining PID parameters through debugging, and obtaining a corresponding dynamic front wheel deflection angle marked as w 1 。
w 1 ＝Pd ef +Ii ef +Dθ ef 。
S5) is marked as d according to the wheelbase w The static deflection angle value corresponding to the front wheel tire is determined to be w 2 。
S6) calculating a final front wheel deflection angle value.
From the dynamic front wheel yaw angle and the static front wheel yaw angle, the front wheel yaw angle finally used for control is calculated to be w=w 1 +w 2 . The calculation of the yaw angle of the rear wheels is identical to the calculation of the yaw angle of the front wheels.
S7) outputting the final front wheel deflection angle w obtained through calculation to a controller as an actual control value, and controlling the front wheels to deflect according to a set value.
S8) back to step S2), the calculation is performed cyclically. Thus realizing PID tracking control on the front wheels of the vehicle.
The steps of the method for PID tracking control of the rear wheels of the vehicle are consistent with those of the method for PID tracking control of the front wheels, and can be directly substituted and deduced by a person skilled in the art through the content, so that the description is not repeated in the embodiment, and the clear understanding of the technical scheme of the invention by the person skilled in the art is not affected.
In the using process of the AGV, different modes are selected for control on different road sections, namely, the straight line mode control is adopted on the straight line road section, the turning mode control is adopted on the arc road section, the switching is stopped at the connecting point, and the switching is stopped again so as to avoid excessive deviation of the vehicle from the track. As shown in fig. 8, when traveling straight from the left, the vehicle is in the straight mode control, and when moving to the engagement point 1, the vehicle is switched from the straight mode to the turning mode control, and when the vehicle continues to move to the engagement point 2, the vehicle is switched from the turning mode to the straight mode control. The specific description is as follows: taking the center of the connecting line of the front wheel and the rear wheel axle of the vehicle as a reference point, the running track of the reference point is formed by connecting a straight line and an arc, and taking a straight line road section as an example, the mode switching rule is as follows: starting from the straight line section, controlling in a straight line mode; when the datum point moves to the joint point of the linear track and the circular arc track (namely, the joint point 1 is shown as a figure, a circular arc road section is to be entered), the datum point is controlled from a linear mode to a turning mode, and moves along the circular arc; when the reference point continues to move to the point where the arc and the straight line track are connected (i.e. the straight line section is to be entered as shown in the connection point 2), the control is performed by switching from the turning mode to the straight line mode, and the process returns to the step 2 of the straight line mode.
In addition, the AGV in the embodiment is characterized in that the center of a front axle of a front wheel and the center of a rear axle of a rear wheel are respectively provided with a magnetic sensor, and the two magnetic sensors respectively sense magnetic nails on paths, so that the AGV works in running; step 1) and step 1) are to obtain the magnetic navigation positioning of two magnetic sensors; in step 3) and step 3), when both magnetic sensors sense magnetic nails at the same time, the actual value of the transverse error is directly sensed by the magnetic sensors to be used as a correction value to replace the transverse offset error for calculation, and when both magnetic sensors do not sense magnetic nails at the same time, the data of any sensor capable of carrying out relative motion estimation such as an odometer or an inertial measurement unit are acquired, and the calculation of the transverse offset error is carried out based on the latest correction value, namely, the data of both magnetic sensors are used as correction values, and the data of other sensors such as the odometer or the inertial measurement unit are used as prediction values to calculate the transverse error of the front wheel or the rear wheel relative to a reference path.
The above examples and drawings are not intended to limit the form or form of the present invention, and any suitable variations or modifications thereof by those skilled in the art should be construed as not departing from the scope of the present invention.
Claims (6)
1. The magnetic nail positioning arrangement method suitable for double magnetic sensing is characterized in that the vehicle adopting the magnetic nail positioning arrangement mode is characterized in that a chassis system of the vehicle is a chassis system of which front wheels and rear wheels are respectively provided with a front axle and a rear axle which can be independently controlled in steering, the center of the front axle and the center of the rear axle of the vehicle are respectively provided with a magnetic sensor, the magnetic nail positioning arrangement mode comprises a magnetic nail arrangement flow, and the specific magnetic nail arrangement flow is as follows:
1) The method comprises the steps that a center point of a front axle and a rear axle of a vehicle is taken as a datum point, a magnetic nail arrangement route is determined according to a movement track of the datum point, the magnetic nail arrangement route only comprises a straight line and various arcs with fixed radiuses, an arc junction section and a straight line junction section are formed at the junction of the arcs and the straight line in the magnetic nail arrangement route, and two ends of the arc junction section and the straight line junction section are intersection points of the straight line and the arcs;
2) Determining the wheelbase between the front axle and the rear axle, taking the wheelbase as a magnetic nail induction interval distance d, and performing integer division on the magnetic nail induction interval distance d to determine a magnetic nail arrangement interval distance s;
3) Sequentially placing magnetic nails in full segments at equal intervals on a straight line or an arc according to an arrangement interval distance s from a first placement starting point by taking a certain position of the non-straight line junction section of one straight line or a certain position of the non-arc junction section of one arc of the magnetic nail arrangement routes as the first placement starting point;
4) Ensuring that the magnetic nails are placed at two intersection points of the straight line (or the circular arc) and the circular arc (or the straight line);
5) The magnetic nails are placed continuously in the straight line or placed continuously in the straight line, the distance between the first intersection point of the straight line or the straight line in front of the straight line or the straight line is a second placement starting point, the magnetic nails are sequentially placed on the continuous straight line or the straight line according to the placement distance s from the second placement starting point, the intersection point distance ss is larger or smaller than the placement distance s, and the magnetic nails on the two sections of the boundary section of the straight line and the boundary section of the straight line are ensured to be staggered.
2. An AGV tracking control method on a path for arranging magnetic nails by a magnetic nail positioning arrangement method suitable for double magnetic sensing as claimed in claim 1, wherein two magnetic sensors on an AGV vehicle sense the magnetic nails on the path respectively, the tracking control method comprises the steps of marking the tracking control of a straight line path as a straight line mode and marking the tracking control of a turning path as a turning mode, the tracking control of the straight line mode and the turning mode is performed by the tracking control comprising a front wheel and a rear wheel, the tracking control method is described by taking the tracking control of the front wheel as an example, the step of the performing method of the rear wheel is obtained by referring to the front wheel equally, and the steps of the front wheel tracking control method of the straight line mode and/or the turning mode are as follows:
straight line mode:
1) Obtaining magnetic navigation positioning;
2) The given linear velocity is a fixed value;
3) Calculating to obtain a transverse deviation error, an accumulated transverse deviation error and a heading error of a front axle center of the front wheel to an arc path;
4) Determining a PID coefficient, and calculating a dynamic front wheel deflection angle value corresponding to the PID;
5) Determining a final front wheel deflection angle value;
6) The final front wheel deflection angle w obtained through calculation is used as an actual control value to be output to a controller, and the front wheel is controlled to deflect according to a set value;
7) Returning to step 2), the calculation is performed in a loop. Thus realizing PID tracking control on the front wheels of the vehicle;
turning mode:
s1) obtaining magnetic navigation positioning;
s2) the given linear velocity is a fixed value;
s3) calculating to obtain a transverse offset error, an accumulated transverse offset error and a heading error of a front axle center of the front wheel to an arc path;
s4) determining PID coefficients, and calculating dynamic front wheel deflection angle values corresponding to the PID;
s5) determining a static deflection angle value corresponding to the front tire;
s6) determining a final front wheel deflection angle value;
s7) outputting the final front wheel deflection angle w obtained through calculation to a controller as an actual control value, and controlling the front wheel to deflect according to a set value;
s8) returning to the step 2), and circularly performing calculation.
3. The AGV tracking control method according to claim 2, wherein in the straight mode:
step 3) is as follows:
through steps 1) and 2), first, calculate the lateral offset error of the front axle center of the front wheel to the circular arc path and record as d ef And the lateral error is set to be positive when the front wheel is outside the path and negative when the front wheel is inside the path;
next, the accumulated lateral offset error is calculated and noted as i ef The accumulated lateral offset error is the accumulated sum of the lateral offset errors calculated from the start time to the current time;
then, calculate heading error and mark as θ ef The course error is recorded as yaw by taking anticlockwise as positive and the course error as the front wheel course f The reference course is recorded as yaw rf The difference of (a), i.e. θ ef ＝yaw f -yaw rf The reference course is the corresponding tangential direction of the nearest point of the front wheel on the circular arc;
step 4) is to calculate the lateral error d in step 3) ef Cumulative lateral error i ef And heading error θ ef Then, determining PID parameters through debugging, and obtaining a corresponding dynamic front wheel deflection angle w 1 ＝Pd ef +Ii ef +Dθ ef 。
Step 5) is such that the final front wheel yaw angle for control is denoted as w, and the dynamic front wheel yaw angle w obtained in step 4) is 1 ＝Pd ef +Ii ef +Dθ ef As the front wheel yaw value finally used for control, i.e. w=w 1 ；
And/or, in the turning mode:
step S3) is as follows:
through steps S1) and S2), first, calculate the lateral offset error of the front axle center of the front wheel to the circular arc path and record as d ef And the front wheel is set to have a positive lateral error when outside the path and a positive lateral error when inside the pathNegative;
next, the accumulated lateral offset error is calculated and noted as i ef The accumulated lateral offset error is the accumulated sum of the lateral offset errors calculated from the start time to the current time;
then, calculate heading error and mark as θ ef The course error is recorded as yaw by taking anticlockwise as positive and the course error as the front wheel course f The reference course is recorded as yaw rf The difference of (a), i.e. θ ef ＝yaw f -yaw rf The reference course is the corresponding tangential direction of the nearest point of the front wheel on the circular arc;
step S4) is to calculate the lateral error d in step S3) ef Cumulative lateral error i ef And heading error θ ef Then, determining PID parameters through debugging, and obtaining a corresponding dynamic front wheel deflection angle marked as w 1 ，
w 1 ＝Pd ef +Ii ef +Dθ ef ；
Step S5) is recorded as d according to the wheelbase w The static deflection angle value corresponding to the front wheel tire is determined to be w 2 ，
Step 6) calculating the final front wheel deflection angle for control as w=w according to the dynamic front wheel deflection angle and the static front wheel deflection angle 1 +w 2 。
4. The AGV tracking control method according to claim 2 or 3 wherein in both step 1) and step S1) the magnetic navigation positioning of both magnetic sensors is obtained; in the step 3) and the step 3), when the two magnetic sensors sense the magnetic nails at the same time, the real value of the transverse error is directly sensed by the magnetic sensors to be used as a correction value to replace the transverse offset error for calculation, and when the two magnetic sensors do not sense the magnetic nails at the same time, the sensor data of the odometer or the inertial measurement unit are acquired to calculate the transverse offset error based on the latest correction value.
5. The AGV tracking control method according to claim 2 or 3, wherein the AGV vehicle is controlled in different modes in different road segments, in a straight line mode in a straight line segment, in a turning mode in an arc segment, and in an automatic switching mode at a junction point between the straight line segment and the arc segment, the switching mode being defined as follows: taking the center of a connecting line of a front wheel and a rear wheel shaft of the vehicle as a reference point, connecting a running track of the reference point by a straight line and an arc, taking a straight line section as an example, starting from the straight line section, and controlling in a straight line mode; when the datum point moves to the joint point of the linear track and the circular arc track, switching from the linear mode to the turning mode for control, and moving along the circular arc; when the datum point continues to run to the joint point of the circular arc and the linear track, the control is performed by switching from the turning mode to the linear mode.
6. The AGV tracking control method according to claim 4, wherein the AGV vehicle is controlled in different modes in different road segments, in a straight mode in a straight road segment, in a turning mode in an arc road segment, and in an automatic switching mode at a junction point between the straight road segment and the arc road segment, the switching mode being defined as follows: taking the center of a connecting line of a front wheel and a rear wheel shaft of the vehicle as a reference point, connecting a running track of the reference point by a straight line and an arc, taking a straight line section as an example, starting from the straight line section, and controlling in a straight line mode; when the datum point moves to the joint point of the linear track and the circular arc track, switching from the linear mode to the turning mode for control, and moving along the circular arc; when the datum point continues to run to the joint point of the circular arc and the linear track, the control is performed by switching from the turning mode to the linear mode.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202410019030.0A CN117864167A (en) | 2024-01-05 | 2024-01-05 | Magnetic nail positioning arrangement method suitable for double magnetic sensing and AGV tracking control method |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202410019030.0A CN117864167A (en) | 2024-01-05 | 2024-01-05 | Magnetic nail positioning arrangement method suitable for double magnetic sensing and AGV tracking control method |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN117864167A true CN117864167A (en) | 2024-04-12 |
Family
ID=90586075
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202410019030.0A Pending CN117864167A (en) | 2024-01-05 | 2024-01-05 | Magnetic nail positioning arrangement method suitable for double magnetic sensing and AGV tracking control method |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN117864167A (en) |
-
2024
- 2024-01-05 CN CN202410019030.0A patent/CN117864167A/en active Pending
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US6721638B2 (en) | AGV position and heading controller | |
| JPH0324605A (en) | Method for controlling travel of moving robot | |
| CN108052107A (en) | A kind of AGV indoor and outdoor complex navigation system and methods for merging magnetic stripe, magnetic nail and inertial navigation | |
| JPH06300581A (en) | Control device for tracking vehicle course | |
| CN111487976B (en) | Backing track tracking method | |
| JPS6319010A (en) | Guiding method for autonomous running vehicle | |
| CA2616613A1 (en) | Guidance, navigation, and control system for a vehicle | |
| CN111522337A (en) | Fuzzy control-based multi-driving-wheel AGV navigation method | |
| JP4264399B2 (en) | Automated guided vehicle | |
| CN112130558B (en) | Navigation system and control method based on differential wheel set AGV | |
| JPH06300580A (en) | Control device for tracking vehicle course | |
| CN117864167A (en) | Magnetic nail positioning arrangement method suitable for double magnetic sensing and AGV tracking control method | |
| US11912360B2 (en) | Vehicle control method, vehicle control system, and vehicle | |
| TWI770966B (en) | Guidance control method of unmanned self-propelled vehicle | |
| JP4269170B2 (en) | Trajectory tracking control method and apparatus | |
| CN117601902A (en) | Dual PID tracking control method suitable for front and rear axle independent steering AGVs | |
| Fnadi et al. | Local obstacle-skirting path planning for a fast bi-steerable rover using bézier curves | |
| Das et al. | Path tracking and control for parallel parking | |
| WO2023054212A1 (en) | Control method and control system | |
| CN110716561A (en) | Sweeper forward-looking pose control method and system for trajectory tracking | |
| WO2023054213A1 (en) | Control method and control system | |
| Raikwar | Robot Navigation and Model Development | |
| Linlin | Research on robot motion control and trajectory tracking based on agricultural seeding | |
| CN118092208A (en) | In-station AGV intelligent navigation method and system based on global visual servo | |
| CN114237214A (en) | Differential AGV control method and system based on fuzzy control and cascade control |
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 |