Method for trajectory tracking control of a magnetically guided vehicle
Patent Information
- Application Number
- CN202611190536.3
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2026-08-06
- Publication Date
- 2026-10-16
AI Technical Summary
[0006]针对现有磁导航车辆难以建立磁感应检测数据与车辆实际目标转向角之间明确对应关系，导致车辆路径跟踪精度较低、控制稳定性较差的问题，本发明提供一种磁导航车辆的轨迹跟踪控制方法
本发明提供的一种磁导航车辆的轨迹跟踪控制方法，通过建立以车体为参考的车辆坐标系，获取多个磁感应传感器检测到的引导磁条对应的检测数据，根据磁感应传感器的安装坐标及检测数据确定引导磁条在车辆坐标系中的局部轨迹，并基于预设前视距离确定车辆追踪点，再结合车辆运动学几何关系计算车辆的理论目标转向角，从而建立了引导磁条检测数据与车辆目标转向角之间的对应关系。相比于现有技术中直接依据检测偏差进行控制的方式，本发明将车辆轨迹跟踪过程转化为基于局部轨迹和追踪点的几何计算过程，使车辆目标转向角具有明确的计算依据，提高了轨迹跟踪控制的准确性和可靠性。
Smart Images
Figure CN122923852A_ABST
Abstract
Description
Technical Field
[0001] This invention relates to the field of vehicle navigation control algorithms, and more specifically to a trajectory tracking control method for magnetic navigation vehicles. Background Technology
[0002] Magnetic navigation vehicles are automated guidance devices that track paths by detecting pre-embedded magnetic strips (or other magnetic guiding media) on the ground. They are widely used in industrial logistics, warehousing and handling, intelligent manufacturing, and assisted driving. During vehicle operation, the onboard magnetic induction device detects the positional relationship between the vehicle and the guiding magnetic strip in real time, and controls the vehicle's steering based on the detection results, allowing the vehicle to travel along the guiding magnetic strip.
[0003] Current control methods for magnetically guided vehicles typically calculate the vehicle's steering control parameters based on deviation information detected by a magnetic induction device to achieve vehicle tracking of the guide magnetic strip. However, the detected deviation information only reflects the relative positional relationship between the vehicle and the guide magnetic strip. The actual steering angle that the vehicle needs to execute is not only related to the detected deviation but also closely related to the vehicle's attitude, kinematic characteristics, and the spatial relationship of the guide path. Therefore, accurately determining the target steering angle that the vehicle should execute under the current operating conditions based on the detected information is a key issue in the path tracking control of magnetically guided vehicles.
[0004] On the other hand, during long-term use, actual vehicles are affected by factors such as machining errors, assembly errors, transmission clearances, tire wear, and steering mechanism zero-point drift. This often results in a certain deviation between the actual vehicle motion state and the theoretical motion model. Simultaneously, vehicles are also affected by random disturbances such as uneven road surfaces and mechanical vibrations, causing deviations between the actual vehicle performance and the theoretical control results. When these factors work together, it can easily lead to decreased vehicle tracking accuracy and poorer operational smoothness. In severe cases, it can even cause the vehicle to deviate from the guide path, thus affecting the stability and reliability of vehicle operation.
[0005] Therefore, how to establish a more accurate and clear correspondence between detection information and vehicle target steering angle, reduce the impact of vehicle motion model errors and external disturbances on control performance, and improve the path tracking accuracy and operational stability of magnetic navigation vehicles has become a technical problem that urgently needs to be solved by those skilled in the art. Summary of the Invention
[0006] To address the problem of low path tracking accuracy and poor control stability in existing magnetic navigation vehicles due to the difficulty in establishing a clear correspondence between magnetic induction detection data and the vehicle's actual target steering angle, this invention provides a trajectory tracking control method for magnetic navigation vehicles. This method constructs a local trajectory of the guide magnetic strip in the vehicle coordinate system based on information from multiple magnetic induction sensors, and calculates the vehicle's theoretical target steering angle by combining this information with the vehicle's kinematic geometry. This enables continuous and stable tracking of the guide magnetic strip by the vehicle, improving the path tracking accuracy and operational stability of the magnetic navigation vehicle.
[0007] To achieve the above objectives, the present invention provides a trajectory tracking control method for a magnetically navigated vehicle, comprising the following steps: S1. Establish a vehicle coordinate system with the vehicle body as a reference; and obtain the installation coordinates of at least two magnetic induction sensors on the vehicle body, as well as the detection data corresponding to the same guide magnetic strip detected by each magnetic induction sensor; S2. Based on the installation coordinates and detection data of each magnetic induction sensor, determine the local trajectory of the guide magnetic strip in the vehicle coordinate system; S3. Determine the vehicle tracking point on the local trajectory based on the preset forward viewing distance; S4. Calculate the theoretical target steering angle of the vehicle based on the tracking point and the vehicle's kinematic geometry. S5. Control the vehicle steering according to the theoretical target steering angle, and repeat steps S1 to S5 according to a preset control cycle so that the vehicle continuously tracks the guide magnetic strip.
[0008] In some implementations, in step S2, at least two detection points on the guide magnetic strip are determined based on the installation coordinates of each magnetic induction sensor and the corresponding detection data, and a local trajectory of the guide magnetic strip is established based on the at least two detection points. The local trajectory is a local straight line obtained by fitting two detection points, and its expression is:
[0009] in, , These are the longitudinal coordinates of the two magnetic induction sensors in the vehicle coordinate system; , These are the horizontal coordinates of the corresponding detection points.
[0010] In some implementations, in step S3, a forward-looking circle is established with the origin of the vehicle coordinate system as the center and the forward-looking distance as the radius. The intersection point of the forward-looking circle and the local trajectory is determined, and the intersection point located in front of the vehicle's driving direction is determined as the tracking point. When the forward-looking circle and the local trajectory have two intersection points, the intersection point with the larger x-coordinate value is selected as the tracking point.
[0011] In some implementations, the forward look-ahead distance These values can be fixed preset values or dynamically adjusted based on the vehicle's operating status. Current viewing distance When dynamically adjusted, the forward sight distance is calculated according to the following formula:
[0012] in, This is the operating condition coefficient; The velocity coefficient; For vehicle operating speed; Based on the forward sight distance.
[0013] In some implementations, in step S4, the center of the vehicle's rear axle is used as the origin of the vehicle coordinate system. Using the tracking point as the target point ; With a point on the Y-axis As the instantaneous center of rotation of the vehicle, the center of the rear axle of the vehicle Tracking points and instantaneous rotation center Form an isosceles triangle that satisfies:
[0014] Based on the geometric relationships of the isosceles triangle, we can obtain:
[0015] in, The angle between the tracking point and the longitudinal centerline of the vehicle.
[0016] In some implementations, the distance from the rear axle center to the instantaneous rotation center is determined based on the geometric relationship between the vehicle's rear axle center, the tracking point, and the instantaneous rotation center, satisfying the following:
[0017] Further results were obtained:
[0018] in, It is the distance from the center of the rear axle of the vehicle to the instantaneous center of rotation.
[0019] In some implementations, the theoretical target steering angle is calculated based on the vehicle wheelbase and the geometric relationship between the vehicle's front wheel steering axis center, the vehicle's rear axle center, and the instantaneous rotation center, satisfying:
[0020] in, This refers to the vehicle's wheelbase. The theoretical target steering angle; Combination:
[0021] as well as:
[0022] The theoretical target steering angle is obtained as follows:
[0023] in,( , () represents the coordinates of the tracking point in the vehicle coordinate system.
[0024] In some implementations, the theoretical target steering angle is corrected by constructing integral and differential compensation terms based on the lateral offset of the tracking point relative to the longitudinal centerline of the vehicle. The integral compensation term satisfies:
[0025] The differential compensation term satisfies:
[0026] in, This represents the lateral offset of the tracking point in the current control cycle.
[0027] In some implementations, the corrected target steering angle satisfy:
[0028] in, This is the integral compensation coefficient; is the differential compensation coefficient.
[0029] In some implementations, steps S1 to S5 are executed cyclically according to a preset control cycle. In each control cycle, the magnetic induction sensor detection data is reacquired, the local trajectory of the guide magnetic strip is updated, the tracking point is re-determined, the theoretical target steering angle is recalculated, and the vehicle is controlled to continuously track the guide magnetic strip. When integral and derivative compensation are enabled, the corrected target steering angle is calculated based on the theoretical target steering angle. Furthermore, integral and derivative compensation must be executed cyclically according to a preset control cycle.
[0030] The trajectory tracking control method for a magnetic navigation vehicle, applying the above-mentioned technical solution of the present invention, has the following effects: This invention provides a trajectory tracking control method for magnetically guided vehicles. By establishing a vehicle coordinate system with the vehicle body as a reference, it acquires detection data corresponding to guide magnetic strips detected by multiple magnetic induction sensors. Based on the installation coordinates of the magnetic induction sensors and the detection data, it determines the local trajectory of the guide magnetic strip in the vehicle coordinate system and determines the vehicle tracking point based on a preset forward-looking distance. Then, it calculates the vehicle's theoretical target steering angle by combining the vehicle's kinematic geometry, thus establishing a correspondence between the guide magnetic strip detection data and the vehicle's target steering angle. Compared to existing technologies that directly control based on detection deviations, this invention transforms the vehicle trajectory tracking process into a geometric calculation process based on the local trajectory and the tracking point, providing a clear calculation basis for the vehicle's target steering angle and improving the accuracy and reliability of trajectory tracking control.
[0031] Meanwhile, this invention cyclically executes steps such as acquiring detection data, updating local trajectory, determining tracking points, and calculating the theoretical target steering angle according to a preset control cycle. This enables the vehicle to continuously update the control target based on real-time changes in the position of the guide magnetic strip, achieving continuous tracking of the guide magnetic strip. When the direction of the guide magnetic strip changes during vehicle movement, this invention can promptly update the vehicle's target steering angle, maintaining stable vehicle movement along the guide magnetic strip. This improves the continuity and operational stability of vehicle trajectory tracking, reduces the risk of the vehicle deviating from the guide path, and has significant engineering application value.
[0032] Other features and advantages of the present invention will be described in detail in the following detailed description section. Attached Figure Description
[0033] Figure 1 This is a simplified schematic diagram of the motion model of the present invention; Figure 2 This is a flowchart of the process of the present invention. Detailed Implementation
[0034] The following provides a detailed description of specific embodiments of the present invention. It should be understood that the specific embodiments described herein are for illustrative and explanatory purposes only and are not intended to limit the scope of the invention.
[0035] In this invention, unless otherwise stated, directional terms such as "upper" and "lower" generally refer to the orientation in the assembled and used state. "Inner" and "outer" refer to the inner and outer sides relative to the outline of each component itself.
[0036] This embodiment provides a trajectory tracking control method for magnetically guided vehicles, applicable to various vehicles using magnetic strips as guide lines, such as AGVs, forklifts, tractors, transport vehicles, and other vehicles with automatic or semi-automatic magnetic navigation functions. This invention does not limit the specific type of vehicle or the specific structure of the magnetic induction sensor, as long as it can detect the positional relationship between the vehicle and the same guide magnetic strip.
[0037] In this embodiment, the vehicle is described using a front-wheel steering and rear-wheel load-bearing structure. Figure 1 As shown, the origin of the vehicle coordinate system is the intersection of the line connecting the rear wheels and the longitudinal centerline of the vehicle. The direction of vehicle movement is defined as The positive direction of the axle and the direction of the rear wheel connection line are defined as follows: The coordinate system is established along the axial direction, thereby creating a vehicle coordinate system with the vehicle body as a reference. It should be understood that for other types of vehicles, it is sufficient to establish a corresponding vehicle coordinate system based on the actual structure of the vehicle; this invention does not limit this.
[0038] At least two magnetic induction sensors are installed on the vehicle body. Preferably, the two magnetic induction sensors are arranged at intervals along the longitudinal centerline of the vehicle and are located at different longitudinal positions in the vehicle coordinate system. The installation coordinates of the two magnetic induction sensors are known parameters after the vehicle is assembled and can be pre-stored in the vehicle controller. In this embodiment, two magnetic induction sensors are used for illustration, and their installation coordinates are denoted as (…). ,0) and ( ,0), where, , These represent the longitudinal coordinates of the two magnetic induction sensors in the vehicle coordinate system.
[0039] It should be noted that this invention does not limit the specific installation location of the magnetic induction sensors, as long as the installation coordinates of each magnetic induction sensor in the vehicle coordinate system can be obtained. The two magnetic induction sensors can be arranged at different positions on the longitudinal centerline of the vehicle, or they can be arranged at other positions according to the actual structure of the vehicle. Accordingly, the corresponding installation coordinates can be used for calculation in the subsequent calculation process.
[0040] During vehicle operation, each magnetic induction sensor continuously detects the position of the same guide magnetic strip and outputs corresponding detection data. Since each magnetic induction sensor can detect the position of the guide magnetic strip relative to itself, the spatial position of the corresponding detection point on the guide magnetic strip in the vehicle coordinate system can be determined based on the installation coordinates of the magnetic induction sensors and the detection data.
[0041] In this embodiment, the detection points corresponding to the guide magnetic strip detected by the two magnetic induction sensors are respectively denoted as... , The magnetic induction sensor detects the lateral distance of the detection point relative to the sensor. Since the longitudinal coordinates of each magnetic induction sensor are known, the detection point can be directly determined. , The coordinates in the vehicle coordinate system are as follows: ,
[0042] in, , These represent the lateral coordinates of the two detection points in the vehicle coordinate system.
[0043] Since both detection points are located on the same guide magnetic strip, a local trajectory of the guide magnetic strip within the current control cycle can be established based on the two detection points. In this embodiment, the local trajectory is approximated as a local straight line, and the expression for the local straight line can be obtained from detection points P1 and P2 as follows:
[0044] This allows for the reconstruction of the local trajectory of the guide magnetic strip within the current control cycle.
[0045] It should be noted that, because the vehicle controller repeatedly executes the method of this invention according to a preset control cycle, the aforementioned local trajectory is not fixed, but is recalculated based on the latest data detected by the magnetic induction sensor in each control cycle. As the vehicle travels along the guide magnetic strip, the vehicle's position relative to the guide magnetic strip changes continuously, and correspondingly, the detection point... , The position is constantly updated, and therefore the local trajectory is also constantly updated. Although this embodiment uses a local straight line to approximate the guide magnetic strip, as the control cycle is repeated, the vehicle controller can obtain the latest local trajectory of the guide magnetic strip in the vehicle coordinate system in real time, thereby realizing continuous tracking of curves and other non-straight paths without having to establish a mathematical model of the entire guide line in advance.
[0046] After obtaining the local trajectory of the guide magnetic strip, the next step is to determine the tracking point corresponding to the current control cycle of the vehicle. This invention uses a forward-looking distance method to determine the tracking point; specifically, with the origin O of the vehicle coordinate system as the center, a preset forward-looking distance is used. Establish a forward-looking circle with a radius, and calculate the intersection point between the forward-looking circle and the aforementioned local trajectory. Typically, the forward-looking circle and the local trajectory have two intersection points: one located in front of the vehicle's direction of travel, and the other located behind the vehicle. To ensure the vehicle always tracks the forward path, this embodiment selects the intersection point located in front of the vehicle's direction of travel as the tracking point P. Furthermore, when using the vehicle coordinate system for calculation, the intersection point with the larger x-coordinate value can be directly selected as the tracking point, while the other intersection point is discarded.
[0047] If you need to control the vehicle to track backwards, you need to discard the tracking points with larger values and select the ones with smaller values.
[0048] Thus, the coordinates of tracking point P in the vehicle coordinate system can be obtained within the current control cycle, denoted as ( , ).in, This represents the longitudinal coordinate of the tracking point relative to the origin of the vehicle coordinate system. This represents the lateral offset of the tracking point relative to the vehicle's longitudinal centerline. It should be noted that the subsequent calculation of the vehicle's theoretical target steering angle, as well as integral and differential compensation, are all based on this tracking point. Therefore, the tracking point not only serves as the vehicle's current geometric tracking target but also as a key computational node in the entire control algorithm. The next section will combine... Figure 1 The geometric derivation process of the theoretical target steering angle of the vehicle is explained in detail.
[0049] After determining the tracking point P corresponding to the current control cycle, this invention further calculates the theoretical target steering angle that the vehicle needs to execute based on the vehicle's kinematic geometry. It should be noted that this invention does not directly control the vehicle's steering based on the deviation value detected by the magnetic induction sensor. Instead, it first establishes the geometric relationship between the vehicle and the guide magnetic strip, and then calculates the theoretical target steering angle that the vehicle should execute based on the vehicle's kinematic model, thereby establishing a clear mathematical correspondence between the detection data and the vehicle's steering execution amount.
[0050] like Figure 1 As shown, in this embodiment, the center of the rear axle of the vehicle is taken as the origin O of the vehicle coordinate system, the center of the steering axle of the front wheels is denoted as point A, and the wheelbase of the vehicle is denoted as L. That is, the coordinates of point A in the vehicle coordinate system are (L, 0). The tracking point P has been determined according to the aforementioned steps, and its coordinates are (L, 0). , ).
[0051] In order for the vehicle to gradually approach the tracking point P along a smooth curved path, this embodiment requires the use of the vehicle's instantaneous rotation center C based on the characteristics of the vehicle's steering kinematics model.
[0052] Specifically, there exists a point C on the Y-axis of the vehicle coordinate system as the instantaneous rotation center of the vehicle. This point satisfies the following conditions: the rear axle center O, the tracking point P, and the instantaneous rotation center C.
[0053] That is, △OCP is an isosceles triangle.
[0054] The above geometric relationship means that the vehicle takes the rear axle center O as a reference and moves in an arc with the instantaneous rotation center C as the center within the current control cycle. The rear axle center O and the tracking point P are both located on this arc, so that the vehicle can gradually approach the target path by continuously turning, rather than moving directly to the target point in a broken line.
[0055] Let α be the angle between the tracking point P and the vehicle's longitudinal centerline. Then α is the angle between line segment OP and the vehicle's X-axis. According to the vehicle coordinate system, we know that:
[0056] Since △OCP is an isosceles triangle, we can conclude that:
[0057] Right now:
[0058] This allows us to establish the geometric relationship between the tracking point location and the vehicle's instantaneous rotation center.
[0059] Furthermore, in △OCP, according to the Law of Sines, we can obtain:
[0060] because
[0061] and
[0062] Therefore, further simplification yields:
[0063] OC is the distance from the rear axle center to the instantaneous rotation center of the vehicle, and it is also the instantaneous turning radius corresponding to the vehicle's current trajectory.
[0064] After calculating the instantaneous turning radius of the vehicle, a vehicle steering angle calculation model can be further established by combining the vehicle steering mechanism.
[0065] like Figure 1 As shown, the center of the vehicle's front wheel steering axis is point A. In triangle OCA, since OA and OC are perpendicular to each other, therefore:
[0066] Since the theoretical target steering angle δ of the vehicle is the deflection angle of the front wheels relative to the longitudinal centerline of the vehicle, we can obtain:
[0067] Further results were obtained:
[0068] Since the vehicle wheelbase OA = L, and considering the aforementioned:
[0069] Substituting into the above equation, we get:
[0070] Furthermore, based on the coordinates of tracking point P in the vehicle coordinate system, we can obtain:
[0071] at the same time,
[0072] Substituting the above relationship into the expression for the vehicle's theoretical target steering angle, we get:
[0073] Where L is the vehicle wheelbase; , ) represents the coordinates of the tracking point in the vehicle coordinate system; δ represents the theoretical target steering angle calculated in the current control cycle.
[0074] As can be seen from the above calculation process, the theoretical target steering angle δ is determined by both the vehicle structural parameters and the current tracking point position. Among these, the vehicle wheelbase L is an inherent parameter of the vehicle and remains unchanged after the vehicle is determined; while the tracking point coordinates (… , The value changes continuously as the data detected by the magnetic induction sensor is updated. Therefore, in each control cycle, the corresponding theoretical target steering angle can be recalculated based on the relative position between the vehicle's current position and the guide magnetic strip, ensuring that the vehicle's trajectory always matches the local trajectory of the guide magnetic strip.
[0075] It should be noted that the theoretical target steering angle mentioned above is calculated under ideal conditions where the vehicle motion model is accurate and there are no systematic errors in the vehicle steering actuator. For magnetic navigation vehicles with precisely calibrated vehicle motion model parameters, good trajectory tracking performance can be achieved solely based on the above theoretical target steering angle.
[0076] However, in practical engineering applications, especially for magnetic navigation vehicles used to assist manual driving, there is often a deviation between the actual steering angle and the theoretical target steering angle due to unavoidable factors such as mechanical clearances, assembly errors, steering mechanism zero-position deviations, and tire wear during long-term vehicle use, as well as external disturbances such as road surface undulations and mechanical vibrations during vehicle operation. If the vehicle steering is always directly controlled according to the theoretical target steering angle, the vehicle is prone to running towards the guide magnetic strip side for a long time, increasing the risk of the vehicle deviating from the guide path or even derailing.
[0077] Therefore, based on the theoretical target steering angle, this invention further introduces integral and differential compensation terms to correct the theoretical target steering angle, thereby reducing the impact of vehicle motion model errors and external disturbances on vehicle trajectory tracking performance. The following section, in conjunction with the control flow of this invention, details the construction methods of the integral and differential compensation terms and the target steering angle correction process.
[0078] As mentioned earlier, the theoretical target steering angle δ calculated based on the vehicle's kinematic geometry is obtained under ideal conditions where the vehicle's motion model is accurate and the steering actuator has no systematic errors. For rigorously calibrated automated navigation vehicles, a good trajectory tracking effect can be achieved based on the theoretical target steering angle. However, for vehicles that extensively use magnetic navigation to assist manual driving, the actual steering angle executed by the vehicle usually deviates from the theoretical target steering angle issued by the control system due to factors such as machining errors, assembly errors, steering mechanism clearance, tire wear, and steering zero-point drift during long-term use. Furthermore, the vehicle is also affected by random disturbances such as uneven road surfaces and vehicle vibrations during operation, causing continuously changing noise in the trajectory tracking process. All of these factors affect the vehicle's trajectory tracking accuracy and may even cause the vehicle to continuously deviate from the guide magnetic strip.
[0079] To address the aforementioned issues, this invention, based on the theoretical target steering angle δ, only introduces integral and differential compensation terms to correct the theoretical target steering angle. Since the previous algorithm has already completed the proportional control function between the vehicle's current position and the target trajectory, this invention does not need to set a proportional term. Instead, it utilizes the integral term to eliminate long-standing systematic errors and the differential term to suppress instantaneous fluctuations caused by random disturbances, thereby improving the stability of vehicle trajectory tracking.
[0080] In this embodiment, both the integral and differential terms are expressed as the lateral offset of the tracking point P relative to the longitudinal centerline of the vehicle. As input. It should be noted that here... It is not the detection error of the magnetic induction sensor itself, but the lateral coordinate of the tracking point in the vehicle coordinate system constructed based on the current control cycle. It can more accurately reflect the degree of lateral deviation between the current position of the vehicle and the target trajectory. Therefore, using this parameter as the input of the integral and derivative terms is more consistent with the actual motion state of the vehicle.
[0081] Specifically, in the nth control cycle, the integral compensation term can be expressed as:
[0082] in, This represents the lateral offset of the tracking point corresponding to the t-th control cycle.
[0083] The integral compensation term reflects the cumulative lateral deviation of the vehicle over multiple consecutive control cycles. When the vehicle continuously deviates towards the guide magnetic strip due to factors such as steering zero-position deviation, mechanical clearance, or tire wear, the integral compensation term accumulates, gradually increasing the corresponding compensation amount to correct the theoretical target steering angle, allowing the vehicle to gradually return to the vicinity of the guide magnetic strip. Therefore, the integral compensation term is mainly used to compensate for the persistent systematic errors inherent in the vehicle itself, improving the trajectory tracking accuracy during long-term vehicle operation.
[0084] Meanwhile, the present invention further constructs a differential compensation term, the expression of which is:
[0085] in, This indicates the lateral offset of the tracking point in the current control cycle; This indicates the lateral offset of the tracking point in the previous control cycle.
[0086] The differential compensation term reflects the changing trend of the lateral offset between two adjacent control cycles. When the vehicle is affected by external factors such as road surface undulations and mechanical vibrations, the lateral offset of the tracking point usually changes rapidly. At this time, the differential compensation term can reflect the rate of change of the lateral offset in a timely manner, making early corrections to the vehicle steering control, thereby reducing oscillations during vehicle steering, improving the smoothness of vehicle operation, and reducing the impact of random noise on the vehicle control system.
[0087] Based on the aforementioned integral and differential compensation terms, the present invention ultimately outputs the target steering angle to the vehicle steering actuator. satisfy:
[0088] in, This is the integral compensation coefficient; is the differential compensation coefficient.
[0089] The control process described above in this invention is executed cyclically according to the control cycle set by the vehicle controller. Specifically, in each control cycle, the controller reacquires the guide magnetic strip position data detected by each magnetic induction sensor, re-determines the coordinates of the detection point based on the latest detection data, refits the local trajectory of the guide magnetic strip, re-determines the tracking point position, recalculates the theoretical target steering angle, and calculates the corrected target steering angle based on the integral compensation term and the differential compensation term. Subsequently, it sends a new steering control command to the vehicle steering actuator.
[0090] As the vehicle continues to move, the above process is repeated continuously. Since the local trajectory is reconstructed using the latest detected information from the guide strip in each control cycle, even if the guide strip is a curved path, the vehicle can always track the direction of the guide strip by continuously updating the local trajectory and tracking points, without needing to pre-build a complete mathematical model of the entire guide path. As the vehicle approaches the target path, the tracking point position and the theoretical target steering angle are continuously updated, and the vehicle eventually achieves stable continuous movement along the guide strip.
[0091] Furthermore, in this invention, the forward sight distance... Different determination methods can be adopted depending on the different application scenarios.
[0092] In one implementation, the forward look-ahead distance A fixed preset value is used. The vehicle controller is pre-set with a fixed forward sight distance, which remains constant throughout the entire operation. Within each control cycle, the controller uses the origin of the vehicle coordinate system as the center and maintains this fixed forward sight distance. A forward-looking circle is established with a radius, and the intersection point between the forward-looking circle and the local trajectory of the guide magnetic strip located in front of the vehicle is determined as the tracking point. This implementation method has a simple calculation process and is suitable for application scenarios where the vehicle speed changes little and the road conditions are relatively stable.
[0093] In another implementation, to balance stability at high speeds and tracking accuracy at low speeds, the forward sight distance is dynamically adjusted, satisfying the following:
[0094] in, This is the operating condition coefficient; The velocity coefficient; The current operating speed of the vehicle Based on the forward sight distance.
[0095] The vehicle's operating speed can be obtained from the rotational speed feedback of the vehicle's drive motor or from the vehicle's speed sensor; this invention does not limit the specific speed.
[0096] Operating condition coefficient The settings are based on the vehicle's current operating environment. For example, when the vehicle is running on a straight road, the operating condition coefficient can be appropriately increased to increase the forward sight distance, thereby reducing the vehicle's target steering angle and improving the vehicle's running smoothness. When the vehicle enters a curve or other operating conditions that require improved path tracking accuracy, the operating condition coefficient can be appropriately decreased to reduce the forward sight distance, thereby improving the vehicle's tracking accuracy of the guide magnetic strip. The operating condition coefficient can be automatically switched based on the vehicle navigation system's identification of the current path type, or it can be adjusted by the vehicle control system based on external trigger signals; this invention does not limit this adjustment.
[0097] speed coefficient This is used to establish the correspondence between vehicle speed and forward sight distance. As vehicle speed increases, forward sight distance increases accordingly, allowing the vehicle to plan its trajectory in advance and reducing sudden steering changes at high speeds. When vehicle speed decreases, forward sight distance decreases accordingly, allowing the vehicle to track the guide strip more accurately and improving control precision at low speeds.
[0098] Therefore, the present invention can adaptively adjust the position of the tracking point according to the vehicle's operating status, so that the vehicle can maintain a good trajectory tracking effect under different speeds and different operating conditions.
[0099] In summary, the complete control process of this invention is as follows: First, a vehicle coordinate system with the vehicle body as a reference is established, and the installation coordinates and detection data of at least two magnetic induction sensors are acquired. Then, a local trajectory of the guide magnetic strip in the vehicle coordinate system is constructed based on the detection points obtained by the magnetic induction sensors. Next, a forward-looking circle is established based on a preset forward-looking distance to determine the tracking point on the local trajectory. Further, the theoretical target steering angle of the vehicle is calculated based on the vehicle's kinematic geometry. For vehicles that require error compensation, integral compensation terms and differential compensation terms are constructed based on the lateral offset of the tracking point to correct the theoretical target steering angle, resulting in the final target steering angle output to the vehicle steering actuator. The controller outputs corresponding control commands to the vehicle steering actuator to drive the vehicle to complete the steering control of the current control cycle.
[0100] After the vehicle completes one control cycle, the controller enters the next control cycle, acquires the detection data from the magnetic induction sensor again, and re-executes steps such as local trajectory construction, tracking point determination, theoretical target steering angle calculation, and target steering angle correction, thus forming a continuous closed-loop control process. As the vehicle continues to move forward, the local trajectory of the guide magnetic strip in the vehicle coordinate system is constantly updated, the tracking point position changes continuously, and the target steering angle is also updated in real time, ensuring that the vehicle always runs continuously and stably along the guide magnetic strip. Because the local trajectory is reconstructed based on the latest detection data in each control cycle, this invention is not only applicable to straight paths but also to curves and other continuously changing guide paths, achieving continuous tracking control without the need to pre-establish a mathematical model of the entire guide path.
[0101] The preferred embodiments of the present invention have been described in detail above. However, the present invention is not limited to the specific details in the above embodiments. Within the scope of the technical concept of the present invention, various simple modifications can be made to the technical solution of the present invention, and these simple modifications all fall within the protection scope of the present invention.
[0102] It should also be noted that the various specific technical features described in the above embodiments can be combined in any suitable manner without contradiction. To avoid unnecessary repetition, the present invention will not describe the various possible combinations separately.
[0103] Furthermore, various different embodiments of the present invention can be combined in any way, as long as they do not violate the spirit of the present invention, they should also be regarded as the content disclosed by the present invention.
Claims
1. A trajectory tracking control method for a magnetically navigated vehicle, characterized in that, Includes the following steps: S1. Establish a vehicle coordinate system with the vehicle body as a reference; and obtain the installation coordinates of at least two magnetic induction sensors on the vehicle body, as well as the detection data corresponding to the same guide magnetic strip detected by each magnetic induction sensor; S2. Based on the installation coordinates and detection data of each magnetic induction sensor, determine the local trajectory of the guide magnetic strip in the vehicle coordinate system; S3. Determine the vehicle tracking point on the local trajectory based on the preset forward viewing distance; S4. Calculate the theoretical target steering angle of the vehicle based on the tracking point and the vehicle's kinematic geometry. S5. Control the vehicle steering according to the theoretical target steering angle, and repeat steps S1 to S5 according to a preset control cycle so that the vehicle continuously tracks the guide magnetic strip.
2. The trajectory tracking control method for a magnetic navigation vehicle according to claim 1, characterized in that: In step S2, at least two detection points on the guide magnetic strip are determined based on the installation coordinates of each magnetic induction sensor and the corresponding detection data, and a local trajectory of the guide magnetic strip is established based on the at least two detection points. The local trajectory is a local straight line obtained by fitting two detection points, and its expression is: in, , These are the longitudinal coordinates of the two magnetic induction sensors in the vehicle coordinate system; , These are the horizontal coordinates of the corresponding detection points.
3. The trajectory tracking control method for a magnetic navigation vehicle according to claim 1, characterized in that: In step S3, a forward-looking circle is established with the origin of the vehicle coordinate system as the center and the forward-looking distance as the radius. The intersection point of the forward-looking circle and the local trajectory is determined, and the intersection point located in front of the vehicle's driving direction is determined as the tracking point. When the forward-looking circle and the local trajectory have two intersection points, the intersection point with the larger x-coordinate value is selected as the tracking point.
4. The trajectory tracking control method for a magnetic navigation vehicle according to claim 3, characterized in that: The forward sight distance These values can be fixed preset values or dynamically adjusted based on the vehicle's operating status. Current viewing distance When dynamically adjusted, the forward sight distance is calculated according to the following formula: in, This is the operating condition coefficient; The velocity coefficient; For vehicle operating speed; Based on the forward sight distance.
5. The trajectory tracking control method for a magnetic navigation vehicle according to claim 1, characterized in that: In step S4, the center of the rear axle of the vehicle is taken as the origin of the vehicle coordinate system. Using the tracking point as the target point ; With a point on the Y-axis As the instantaneous center of rotation of the vehicle, the center of the rear axle of the vehicle Tracking points and instantaneous rotation center Form an isosceles triangle that satisfies: Based on the geometric relationships of the isosceles triangle, we can obtain: in, The angle between the tracking point and the longitudinal centerline of the vehicle.
6. The trajectory tracking control method for a magnetic navigation vehicle according to claim 5, characterized in that: The distance from the rear axle center to the instantaneous rotation center is determined based on the geometric relationship between the vehicle's rear axle center, the tracking point, and the instantaneous rotation center, satisfying the following: Further results were obtained: in, It is the distance from the center of the rear axle of the vehicle to the instantaneous center of rotation.
7. The trajectory tracking control method for a magnetic navigation vehicle according to claim 6, characterized in that: The theoretical target steering angle is calculated based on the vehicle's wheelbase and the geometric relationship between the vehicle's front wheel steering axis center, rear axle center, and instantaneous rotation center, satisfying the following: in, This refers to the vehicle's wheelbase. The theoretical target steering angle; Combination: as well as: The theoretical target steering angle is obtained as follows: in,( , () represents the coordinates of the tracking point in the vehicle coordinate system.
8. The trajectory tracking control method for a magnetic navigation vehicle according to claim 7, characterized in that: The theoretical target steering angle is corrected by constructing integral and differential compensation terms based on the lateral offset of the tracking point relative to the longitudinal centerline of the vehicle. The integral compensation term satisfies: The differential compensation term satisfies: in, This represents the lateral offset of the tracking point in the current control cycle.
9. The trajectory tracking control method for a magnetic navigation vehicle according to claim 8, characterized in that: Corrected target steering angle satisfy: in, This is the integral compensation coefficient; is the differential compensation coefficient.
10. The trajectory tracking control method for a magnetic navigation vehicle according to any one of claims 1 to 9, characterized in that: Steps S1 to S5 are executed cyclically according to a preset control cycle. In each control cycle, the magnetic induction sensor detection data is reacquired, the local trajectory of the guide magnetic strip is updated, the tracking point is re-determined, and the theoretical target turning angle is recalculated. And control the vehicle to continuously track the guide magnetic strip; When integral compensation and differential compensation are enabled, the corrected target steering angle is calculated based on the theoretical target steering angle.