Trajectory control method, device and equipment of water navigation vehicle and vehicle

Abstract

The application provides a trajectory control method, device, equipment and vehicle of a water navigation vehicle. It relates to the technical field of vehicle control. The method comprises the following steps: when the vehicle is navigating in water, parameters such as the vehicle wheelbase, the distance from the center of mass to the rear axle, the front wheel turning angle and the vehicle speed are combined to determine a target heading direction that is more in line with the current motion state, and the target heading direction is compared with the actual heading direction to obtain a heading deviation, and then the driving device output is adjusted according to the deviation to correct the navigation trajectory. By using the method, the accuracy of target heading direction determination and the pertinence of trajectory correction can be improved, and the trajectory stability, control accuracy and working condition adaptability of the vehicle in a complex water environment can be improved.

Description

Technical Field

[0001] This application relates to the field of vehicle control technology, and in particular to a trajectory control method, device, equipment and vehicle for underwater vehicles. Background Technology

[0002] When vehicles navigate in water, their operating environment differs significantly from that on land. The dynamic disturbances of water currents, the random effects of crosswinds, and the unique power transmission characteristics of underwater propulsion systems continuously interfere with the vehicle's course, causing deviations in its trajectory. If these deviations are not effectively corrected in a timely manner, they will gradually accumulate and cause the vehicle to stray from its intended path, potentially leading to collisions, grounding, and other safety hazards. Therefore, precise control of the trajectory of vehicles navigating in water is crucial for ensuring their safe navigation on water.

[0003] In related technologies, the heading angle of a vehicle navigating in the water is typically adjusted manually to ensure its trajectory conforms to the desired course. When the vehicle's trajectory deviates, the driver needs to rely on their experience to determine the degree of deviation and correct the trajectory accordingly. However, this method relies excessively on the driver's visual perception and personal experience, making it prone to misjudgment and resulting in low accuracy in trajectory correction, thus affecting navigation safety. Summary of the Invention

[0004] This application provides a trajectory control method, device, equipment, and vehicle for underwater vehicles, which enables automatic and precise correction of the navigation trajectory of underwater vehicles, thereby improving the accuracy of trajectory correction and navigation safety.

[0005] In a first aspect, this application provides a trajectory control method for a vehicle navigating in water, comprising:

[0006] When a vehicle is navigating in water, the heading deviation is calculated based on the actual heading angle and the target heading angle. The target heading angle is calculated based on the vehicle's wheelbase, the distance from the center of gravity to the rear axle, the front wheel steering angle, and the vehicle speed.

[0007] Based on the heading deviation, the vehicle's drive system is controlled to correct the vehicle's trajectory.

[0008] In one possible implementation, the heading deviation includes a current heading deviation calculated based on the current actual heading angle and the current target heading angle;

[0009] The method of controlling the vehicle's drive system based on the heading deviation includes:

[0010] If the absolute value of the current heading deviation is greater than the first preset threshold, then the first power compensation amount is determined based on the current heading deviation and the current target heading angle.

[0011] Based on the first power compensation amount, the vehicle's drive unit is controlled to adjust the vehicle's heading.

[0012] In one possible implementation, the heading deviation also includes the predicted heading deviation at a predicted time after a first preset duration, the predicted heading deviation being calculated based on the predicted actual heading angle and the predicted target heading angle after the first preset duration;

[0013] The drive unit that controls the vehicle's drive system based on the heading deviation further includes:

[0014] If the absolute value of the current heading deviation is less than or equal to the first preset threshold, and the absolute value of the predicted heading deviation is greater than the second preset threshold, the second power compensation amount is determined based on the predicted actual heading angle and the predicted target heading angle.

[0015] The second power compensation amount is corrected according to the power compensation amount correction coefficient to obtain the third power compensation amount. The power compensation amount correction coefficient is calculated based on the angle difference between the current target heading angle and the current actual heading angle.

[0016] Based on the third power compensation amount, the vehicle's drive unit is controlled to adjust the vehicle's heading.

[0017] In one possible implementation, determining the second power compensation amount based on the predicted actual heading angle and the predicted target heading angle includes:

[0018] The difference between the predicted target heading angle and the predicted actual heading angle is calculated to obtain the predicted heading angle deviation;

[0019] Based on the predicted heading angle deviation, the second power compensation amount is calculated using a PID control algorithm.

[0020] In one possible implementation, the method further includes:

[0021] Calculate the angle difference between the current target heading angle and the current actual heading angle;

[0022] Substituting the angle difference into the preset correction coefficient calculation formula, the compensation amount correction coefficient is obtained.

[0023] In one possible implementation, determining the first power compensation amount based on the current heading deviation and the current target heading angle includes:

[0024] Obtain a preset compensation table, which includes power compensation amounts corresponding to different heading deviations and different target heading angles;

[0025] Based on the current heading deviation and the current target heading angle, the compensation table is consulted to obtain the first power compensation amount.

[0026] In one possible implementation, controlling the vehicle's drive unit to adjust the vehicle's heading based on the first power compensation amount includes:

[0027] Based on the vehicle's direction of travel, determine the left-side drive unit and the right-side drive unit;

[0028] If the current heading deviation is positive, the power output of the left drive unit is increased and the power output of the right drive unit is decreased according to the first power compensation amount.

[0029] If the current heading deviation is negative, the power output of the right drive unit is increased and the power output of the left drive unit is decreased according to the first power compensation amount.

[0030] In one possible implementation, controlling the vehicle's drive unit to adjust the vehicle's heading based on the third power compensation amount includes:

[0031] If the predicted heading deviation is positive, the power output of the left drive unit is increased and the power output of the right drive unit is decreased according to the third power compensation amount. The left and right drive units are determined according to the vehicle's forward direction.

[0032] If the predicted heading deviation is negative, the power output of the right drive unit is increased and the power output of the left drive unit is decreased according to the third power compensation amount.

[0033] In one possible implementation, the method further includes:

[0034] Calculate the current target heading angle and the predicted target heading angle based on the vehicle's wheelbase, the distance from the center of gravity to the rear axle, the front wheel steering angle, and the vehicle speed.

[0035] Determine the current actual heading angle based on the GPS signal or the current yaw rate;

[0036] The predicted heading angle is calculated based on the current yaw rate and multiple historical yaw rates within a second preset time period prior to the current moment.

[0037] In one possible implementation, calculating the current target heading angle and the predicted target heading angle based on the vehicle's wheelbase, the distance from the center of gravity to the rear axle, the front wheel steering angle, and the vehicle speed includes:

[0038] The center of gravity sideslip angle is calculated based on the vehicle's wheelbase, the distance from the center of gravity to the rear axle, the front wheel steering angle, and the center of gravity sideslip angle correction coefficient.

[0039] The current instantaneous yaw rate is calculated based on the vehicle's wheelbase, vehicle speed, front wheel steering angle, and yaw rate correction coefficient.

[0040] The product of the current instantaneous yaw rate and the sampling time is determined as the current target yaw angle;

[0041] The predicted target yaw angle is calculated based on the current instantaneous yaw rate and multiple historical instantaneous yaw rates within a second preset time period prior to the current moment;

[0042] The sum of the centroid sideslip angle and the current target yaw angle is determined as the current target heading angle, and the sum of the centroid sideslip angle and the predicted target yaw angle is determined as the predicted target heading angle.

[0043] In one possible implementation, calculating the center-of-gravity sideslip angle based on the vehicle's wheelbase, the distance from the center of gravity to the rear axle, the front wheel steering angle, and the center-of-gravity sideslip angle correction factor includes:

[0044] The ratio of the distance from the center of mass to the rear axle to the wheelbase is defined as the center of mass-wheelbase ratio.

[0045] The product of the front wheel steering angle, the center of gravity wheelbase ratio, and the center of gravity sideslip angle correction coefficient is determined as the center of gravity sideslip angle.

[0046] In one possible implementation, calculating the current instantaneous yaw rate based on the vehicle's wheelbase, vehicle speed, front wheel steering angle, and yaw rate correction coefficient includes:

[0047] The ratio of the vehicle speed to the wheelbase is defined as the speed-wheelbase ratio.

[0048] The product of the front wheel steering angle, the vehicle speed-to-wheelbase ratio, and the yaw rate correction coefficient is determined as the current instantaneous yaw rate.

[0049] In one possible implementation, calculating the predicted target yaw angle based on the current instantaneous yaw rate and multiple historical instantaneous yaw rates within a second preset time period prior to the current moment includes:

[0050] Calculate the integral average of the current instantaneous yaw rate and the multiple historical instantaneous yaw rates to obtain the instantaneous average yaw rate;

[0051] The product of the instantaneous average yaw rate and the first preset duration is determined as the predicted target yaw angle.

[0052] In one possible implementation, determining the current actual heading angle based on the Global Positioning System signal or the current yaw rate includes:

[0053] Determine whether the current confidence level of the Global Positioning System signal is greater than a preset confidence threshold;

[0054] If the current confidence level is greater than the preset confidence threshold, then the current heading angle output by the global positioning system is determined as the current actual heading angle;

[0055] If the current confidence level is less than or equal to the preset confidence threshold, then the current actual heading angle is calculated based on the centroid sideslip angle and the current yaw rate.

[0056] In one possible implementation, calculating the current actual heading angle based on the centroid sideslip angle and the current yaw rate includes:

[0057] The product of the current yaw rate and the sampling time is determined as the change in the current heading angle;

[0058] The sum of the change in the centroid sideslip angle and the change in the current heading angle is determined as the current actual heading angle.

[0059] In one possible implementation, calculating the predicted actual heading angle based on the current yaw rate and multiple historical yaw rates within a second preset time period prior to the current moment includes:

[0060] Calculate the integral average of the current yaw rate and the multiple historical yaw rates to obtain the average yaw rate;

[0061] The product of the average yaw rate and the first preset time is determined as the predicted change in heading angle;

[0062] The sum of the change in the centroid sideslip angle and the predicted heading angle is determined as the predicted actual heading angle.

[0063] Secondly, this application provides a trajectory control device for an underwater vehicle, comprising:

[0064] The first processing module is used to calculate the heading deviation based on the actual heading angle and the target heading angle when the vehicle is navigating in water. The target heading angle is calculated based on the vehicle's wheelbase, the distance from the center of gravity to the rear axle, the front wheel steering angle, and the vehicle speed.

[0065] The second processing module is used to control the vehicle's drive device based on the heading deviation to correct the vehicle's trajectory.

[0066] Thirdly, this application provides a controller, including: a memory and a processor;

[0067] The memory stores computer-executed instructions;

[0068] The processor executes computer execution instructions stored in the memory, causing the processor to perform the first aspect and / or various possible implementations of the first aspect as described above.

[0069] Fourthly, this application provides a vehicle, including: a vehicle body, and a controller as described in the third aspect.

[0070] Fifthly, this application provides a computer-readable storage medium storing computer-executable instructions, which, when executed by a processor, are used to implement the first aspect and / or various possible embodiments of the first aspect.

[0071] In a sixth aspect, this application provides a computer program product, including a computer program that, when executed by a processor, implements the first aspect and / or various possible implementations of the first aspect.

[0072] This application provides a trajectory control method, device, equipment, and vehicle for a vehicle navigating in water. By calculating the heading deviation based on the actual heading angle and the target heading angle during the vehicle's navigation in water, and by correlating the target heading angle with the vehicle's wheelbase, distance from the center of gravity to the rear axle, front wheel steering angle, and vehicle speed, the identification of heading deviation can be made more consistent with the vehicle's current motion characteristics and operating conditions. Furthermore, the vehicle's drive system can be controlled to correct the navigation trajectory based on the heading deviation, improving the timeliness and control accuracy of trajectory correction, thereby enhancing navigation safety. Attached Figure Description

[0073] The accompanying drawings, which are incorporated in and form part of this specification, illustrate embodiments consistent with this application and, together with the description, serve to explain the principles of this application.

[0074] Figure 1 A schematic diagram illustrating the application scenarios provided in the embodiments of this application;

[0075] Figure 2A flowchart illustrating an embodiment of the trajectory control method for underwater vehicles provided in this application;

[0076] Figure 3 A flowchart illustrating Embodiment 2 of the trajectory control method for underwater vehicles provided in this application;

[0077] Figure 4 A flowchart illustrating Embodiment 3 of the trajectory control method for underwater vehicles provided in this application;

[0078] Figure 5 A flowchart illustrating Embodiment 4 of the trajectory control method for underwater vehicles provided in this application;

[0079] Figure 6 A flowchart illustrating Embodiment 5 of the trajectory control method for underwater vehicles provided in this application;

[0080] Figure 7 A flowchart illustrating Embodiment Six of the trajectory control method for underwater vehicles provided in this application;

[0081] Figure 8 A flowchart illustrating Embodiment Seven of the trajectory control method for underwater vehicles provided in this application;

[0082] Figure 9 A schematic diagram of the trajectory control device for an underwater vehicle provided in an embodiment of this application;

[0083] Figure 10 This is a schematic diagram of the controller provided in an embodiment of this application.

[0084] The accompanying drawings illustrate specific embodiments of this application, which will be described in more detail below. These drawings and descriptions are not intended to limit the scope of the concept in any way, but rather to illustrate the concept of this application to those skilled in the art through reference to particular embodiments. Detailed Implementation

[0085] Exemplary embodiments will now be described in detail, examples of which are illustrated in the accompanying drawings. When the following description relates to the drawings, unless otherwise indicated, the same numbers in different drawings denote the same or similar elements. The embodiments described in the following exemplary embodiments do not represent all embodiments consistent with this application. Rather, they are merely examples of apparatuses and methods consistent with some aspects of this application.

[0086] When vehicles travel in water, their operating environment differs significantly from that on land. The irregular currents, the random variations in crosswinds, and the unique dynamic response characteristics of underwater propulsion systems continuously disturb the vehicle's course, causing it to gradually deviate from its intended trajectory. If such deviations are not corrected in time, they accumulate over time, eventually leading to the vehicle straying from its safe course and increasing the risk of collisions, grounding, and other accidents. Therefore, achieving precise control over the trajectory of vehicles navigating in water is crucial for ensuring their safe operation on water.

[0087] To address these issues, existing technologies typically employ manual control. When a vehicle's trajectory deviates from its course in water, the driver relies on their experience to determine the degree of deviation in the heading angle and corrects the trajectory accordingly.

[0088] However, the above methods rely excessively on the driver's visual perception and personal experience, which can easily lead to misjudgments and low accuracy in correcting the navigation trajectory, thus affecting navigation safety. In addition, manual control has a delayed response and cannot simultaneously predict and coordinate corrections for current trajectory deviations and future trajectory deviation trends, making it difficult to cope with dynamic disturbances in complex water environments.

[0089] To address the aforementioned problems, the inventors considered quantifying the heading deviation and generating a target heading angle by combining it with the vehicle's own kinematic parameters. Then, based on the degree of deviation, the drive system was precisely adjusted to achieve automatic and accurate correction of the vehicle's trajectory in water. Based on this, the inventors discovered through numerous experiments that the heading deviation can be calculated based on the vehicle's actual heading angle and the target heading angle while navigating in water. The target heading angle is calculated based on the vehicle's wheelbase, the distance from the center of gravity to the rear axle, the front wheel angle, and the vehicle speed. Subsequently, based on this heading deviation, the vehicle's drive system is controlled to correct the vehicle's trajectory. In this process, by introducing the vehicle's own kinematic parameters to quantify the target heading angle and automatically adjusting the drive system's power output based on the deviation between the actual and target heading angles, the judgment errors caused by traditional manual operation relying on the driver's visual perception and personal experience can be overcome. This achieves accurate and automated correction of the navigation trajectory, effectively suppressing safety hazards such as collisions and grounding caused by trajectory deviation, thereby ensuring the vehicle's navigation safety in water. Based on this, this application proposes a trajectory control method for underwater vehicles to improve the accuracy and automation level of trajectory correction for underwater vehicles.

[0090] Figure 1 This is a schematic diagram illustrating an application scenario provided in an embodiment of this application. Please refer to [link / reference]. Figure 1When a vehicle is navigating in water, the controller deployed in the vehicle corrects the vehicle's trajectory in real time based on the current heading angle and the target heading angle to correct heading deviations and keep the vehicle stably maintaining the expected heading, thereby effectively ensuring navigation safety in water.

[0091] The technical solution of this application and how it solves the above-mentioned technical problems will be described in detail below with specific embodiments. These specific embodiments can be combined with each other, and the same or similar concepts or processes may not be described again in some embodiments. The embodiments of this application will be described below with reference to the accompanying drawings.

[0092] Figure 2 This is a flowchart illustrating an embodiment of the trajectory control method for underwater vehicles provided in this application. Please refer to... Figure 2 The method includes:

[0093] S201. When a vehicle is navigating in water, the heading deviation is calculated based on the actual heading angle and the target heading angle.

[0094] The execution entity in this application embodiment can be a controller, or a trajectory control device for an underwater vehicle installed in the controller. The trajectory control device for the underwater vehicle can be implemented through software or a combination of software and hardware. The trajectory control device for the underwater vehicle can be a processor within the controller. For ease of understanding, the technical solution of this application will be described below using a controller as an example.

[0095] In this step, the controller acquires the vehicle's actual and target heading angles while navigating in water, compares the actual and target heading angles, calculates the difference between them, and determines the heading deviation based on this difference. The target heading angle is calculated based on the vehicle's wheelbase, the distance from the center of gravity to the rear axle, the front wheel steering angle, and the vehicle speed.

[0096] The actual heading angle characterizes the vehicle's current actual direction of travel and is a fundamental parameter for determining whether the vehicle deviates from the desired motion trend. In practice, it can be acquired in real time by sensors (such as GPS, yaw rate sensors, etc.). The target heading angle characterizes the desired direction the vehicle should achieve under the current operating conditions and motion state. Unlike simply assigning a fixed direction based on the driver's steering intention, the target heading angle in this embodiment is calculated jointly using vehicle geometric and dynamic parameters. The wheelbase characterizes the structural distance between the front and rear axles, and the distance from the center of mass to the rear axle reflects the positional relationship of the vehicle's center of mass relative to the rear axle. These two parameters together determine the vehicle's directional response characteristics under steering input. The front wheel steering angle reflects the actual input state of the current steering mechanism. The vehicle speed reflects the intensity of the vehicle's motion and the level of dynamic response when navigating in water.

[0097] The heading deviation is used to quantify the degree of difference between the actual heading angle and the target heading angle, and is the core basis for subsequent drive control.

[0098] S202. Based on the heading deviation, control the vehicle's drive system to correct the vehicle's trajectory.

[0099] In this step, the controller can generate corresponding control commands based on the calculated heading deviation and send the control commands to the vehicle's drive unit to correct the vehicle's trajectory.

[0100] The drive unit may include a left-side drive unit and a right-side drive unit, which are determined according to the current direction of travel of the vehicle.

[0101] In one optional embodiment, the drive unit may include wheel-side motors and / or underwater propulsion. When the vehicle is equipped with wheel-side motors, the controller generates yaw torque to correct the vehicle's heading by controlling the speed difference between the left and right wheel-side motors; when the vehicle is equipped with underwater propulsion, the controller generates yaw torque to correct the vehicle's heading by controlling the thrust difference between the left and right propulsion units; when the vehicle is equipped with both wheel-side motors and underwater propulsion, the controller can coordinate the control of both to work together to correct the vehicle's heading.

[0102] Optionally, the controller can determine the amount and direction of power compensation based on the absolute value and direction of the heading deviation. When the absolute value of the heading deviation is greater than a preset threshold: if the heading deviation is positive, it indicates that the target heading angle is greater than the actual heading angle, and the vehicle's actual heading is deviating to the left. The controller then increases the power output of the left drive unit and decreases the power output of the right drive unit, generating a yaw moment to the right to correct the vehicle's heading to the right. If the heading deviation is negative, it indicates that the target heading angle is less than the actual heading angle, and the vehicle's actual heading is deviating to the right. The controller then increases the power output of the right drive unit and decreases the power output of the left drive unit, generating a yaw moment to the left to correct the vehicle's heading to the left.

[0103] In this embodiment of the application, when the vehicle is navigating in water, the controller can automatically calculate the heading deviation based on the actual heading angle and the target heading angle calculated through the vehicle's kinematic parameters, and automatically control the drive device to correct the navigation trajectory according to the heading deviation. This replaces the traditional manual control method, avoids correction deviations caused by the driver's visual perception and experience judgment, improves the accuracy and automation of trajectory correction, and ensures the vehicle's navigation safety in water.

[0104] Figure 3 This is a flowchart illustrating a second embodiment of the trajectory control method for underwater vehicles provided in this application. Please refer to [link / reference]. Figure 3 Based on the above embodiment one, the heading deviation includes the current heading deviation calculated based on the current actual heading angle and the current target heading angle. The method includes:

[0105] S301. When a vehicle is navigating in water, the current heading deviation is calculated based on the current actual heading angle and the current target heading angle.

[0106] In this step, the controller calculates the difference between the current target heading angle and the current actual heading angle, and multiplies the ratio of this difference to the current target heading angle by 100% to determine the current heading deviation. The current heading deviation characterizes the degree to which the actual heading deviates from the desired heading at the current moment, and is expressed as a percentage.

[0107] S302. If the absolute value of the current heading deviation is greater than the first preset threshold, then the first power compensation amount is determined based on the current heading deviation and the current target heading angle.

[0108] In this step, the controller compares the absolute value of the current heading deviation with a first preset threshold. If the absolute value of the current heading deviation is greater than the first preset threshold, it indicates that the current heading deviation is large and requires rapid intervention for correction. At this time, the controller can determine the first power compensation amount based on the current heading deviation and the current target heading angle.

[0109] Optionally, the first preset threshold can be dynamically determined based on the current vehicle speed and the current target heading angle. Specifically, the higher the current vehicle speed, the smaller the first preset threshold, to avoid the vehicle deviating too much from its trajectory at high speeds and causing danger; the smaller the current target heading angle, the larger the first preset threshold, to avoid the vehicle over-correcting due to excessive sensitivity when turning at small angles.

[0110] Once it is determined that a rapid correction is needed, the controller can obtain a preset compensation table and, based on the current heading deviation and the current target heading angle, query the compensation table to obtain the corresponding first power compensation amount. The compensation table stores the power compensation values ​​corresponding to different heading deviations and different target heading angles.

[0111] For example, a preset compensation scale can be shown in Table 1:

[0112] Table 1

[0113]

[0114] The heading deviation is expressed as a percentage (%), with only positive values ​​shown in the table. The current target heading angle represents the desired heading angle for the vehicle, expressed in degrees (°). The compensation amount represents the first power compensation, expressed in revolutions per minute (rpm), indicating the speed adjustment value of the wheel-side motors or propellers. It should be noted that when the absolute value of the heading deviation is small, the compensation amount is set to 0 to avoid overcompensation; when the current target heading angle is small, the compensation amount can be reduced accordingly to avoid miscompensation. For negative heading deviations, the compensation amount can be the same as for positive deviations, but the compensation direction can be reversed.

[0115] S303. Based on the first power compensation amount, control the vehicle's drive device to adjust the vehicle's heading.

[0116] In this step, the controller controls the drive unit to adjust the vehicle's heading based on the first power compensation amount and the positive or negative direction of the current heading deviation.

[0117] In one specific implementation, the left-side drive unit and the right-side drive unit can be determined based on the vehicle's direction of travel. If the current heading deviation is positive (i.e., the current target heading angle is greater than the current actual heading angle, and the vehicle's actual heading is deviating to the left), the power output of the left-side drive unit is increased and the power output of the right-side drive unit is decreased according to the first power compensation amount. Conversely, if the current heading deviation is negative (i.e., the current target heading angle is less than the current actual heading angle, and the vehicle's actual heading is deviating to the right), the power output of the right-side drive unit is increased and the power output of the left-side drive unit is decreased according to the first power compensation amount.

[0118] For example, taking a vehicle traveling north as an example. If the current heading deviation is positive, that is, the current target heading angle is greater than the current actual heading angle, it indicates that the vehicle's actual heading is deviating to the left of north. The controller then increases the speed of the left drive unit (such as the left wheel-side motor or the left thruster) and decreases the speed of the right drive unit (such as the right wheel-side motor or the right thruster) according to the first power compensation amount, generating a yaw moment to the right, so that the vehicle's heading is corrected to the right and returns to the target heading due north.

[0119] If the current heading deviation is negative, meaning the current target heading angle is less than the current actual heading angle, it indicates that the vehicle's actual heading is deviating to the right of north. The controller then increases the speed of the right drive unit and decreases the speed of the left drive unit based on the first power compensation, generating a yaw moment to the left, causing the vehicle's heading to be corrected to the left until the target heading due north is reached.

[0120] In this embodiment, the controller can calculate the current heading deviation using the current actual heading angle and the current target heading angle. When the deviation exceeds a first preset threshold, it quickly determines the first power compensation amount by consulting a compensation table. Then, based on the positive or negative direction of the deviation, it controls the power output difference between the left and right drive units to generate a corresponding yaw moment, adjusting the vehicle's heading. In this process, the controller achieves rapid response and accurate compensation for the current heading deviation through quantification of the current heading deviation, dynamic threshold judgment, and table lookup compensation mechanism, effectively ensuring the vehicle's navigation safety in water.

[0121] Figure 4 This is a flowchart illustrating Embodiment 3 of the trajectory control method for underwater vehicles provided in this application. Please refer to... Figure 4 Based on the above embodiment two, the heading deviation also includes the predicted heading deviation at the predicted time after a first preset time period. The predicted heading deviation is calculated based on the predicted actual heading angle and the predicted target heading angle after the first preset time period. The method further includes:

[0122] S401. If the absolute value of the current heading deviation is less than or equal to the first preset threshold, and the absolute value of the predicted heading deviation is greater than the second preset threshold, determine the second power compensation amount based on the predicted actual heading angle and the predicted target heading angle.

[0123] In this step, the controller first determines whether the absolute value of the current heading deviation is less than or equal to a first preset threshold, i.e., whether the current heading deviation is within an acceptable range. If this condition is met, the controller can further determine whether the absolute value of the predicted heading deviation is greater than a second preset threshold. When the absolute value of the predicted heading deviation is greater than the second preset threshold, it indicates that although the current heading deviation is not large, the vehicle's heading will show a significant deviation trend in the future, requiring advance predictive correction. At this time, the controller can calculate the predicted heading angle deviation based on the difference between the predicted target heading angle and the predicted actual heading angle, and calculate the second power compensation amount through a PID control algorithm.

[0124] Optionally, the second preset threshold can be dynamically determined based on the vehicle speed and the predicted target heading angle. Specifically, the higher the vehicle speed, the smaller the second preset threshold, to avoid the vehicle deviating too much from its trajectory at high speeds; the smaller the predicted target heading angle, the larger the second preset threshold, to avoid overcorrection during small-angle turns.

[0125] In one specific implementation, after obtaining the predicted heading angle deviation, the second power compensation amount can be calculated by the PID control algorithm, and the calculation formula is shown in the following formula (1):

[0126] Formula (1)

[0127] in, This indicates the second power compensation amount. Indicates the predicted yaw angle. Represents the proportionality coefficient. Represents the integral coefficient. Denotes the differential coefficient. This represents the historical cumulative sum of the predicted heading angle deviation from the initial sampling time to the current sampling time. The above proportionality coefficient represents the difference between the predicted heading angle deviation at the current sampling time and the previous sampling time. Integral coefficient Differential coefficients All of these can be pre-calibrated through real vehicle testing.

[0128] In one specific implementation, the initial sampling time (j=0) refers to the first sampling time when the current prediction correction strategy is activated, that is, the time when the absolute value of the current heading deviation is less than or equal to the first preset threshold and the absolute value of the predicted heading deviation is greater than the second preset threshold. When k=0, the initial sampling time is the current sampling time; when k>0, the initial sampling time is the historical time, and the historical cumulative sum includes all predicted heading angle deviation terms from the activation time to the current time.

[0129] S402. Correct the second power compensation amount according to the power compensation amount correction coefficient to obtain the third power compensation amount.

[0130] In this step, to avoid the prediction correction affecting the heading control at the current moment, a power compensation correction coefficient can be introduced to correct the second power compensation. The power compensation correction coefficient is then multiplied by the second power compensation to obtain the third power compensation.

[0131] The power compensation correction coefficient is determined based on the angular difference between the current target heading angle and the current actual heading angle. Specifically, the controller can calculate the difference between the current target heading angle and the current actual heading angle, and substitute this difference into a preset correction coefficient function to obtain the power compensation correction coefficient.

[0132] Optionally, the correction coefficient function is a monotonically increasing function: the larger the absolute value of the angle difference, the closer the correction coefficient is to 1; the smaller the absolute value of the angle difference, the closer the correction coefficient is to 0. The value range of the correction coefficient is from 0 to 1.

[0133] S403. Based on the third power compensation amount, control the vehicle's drive unit to adjust the vehicle's heading.

[0134] In this step, the controller can adjust the vehicle's heading by controlling the drive unit based on the third power compensation amount and the positive or negative direction of the predicted heading deviation.

[0135] Similarly, the left and right drive units can be determined based on the vehicle's direction of travel. If the predicted heading deviation is positive (i.e., the predicted target heading angle is greater than the predicted actual heading angle, and the predicted vehicle heading is to the left), the power output of the left drive unit can be increased and the power output of the right drive unit can be decreased based on the third power compensation amount, generating a yaw moment to the right, so that the vehicle heading is corrected to the right.

[0136] Conversely, if the predicted heading deviation is negative (i.e., the predicted target heading angle is less than the predicted actual heading angle, and the predicted vehicle heading is deviated to the right), the power output of the right drive unit can be increased and the power output of the left drive unit can be decreased according to the third power compensation amount, generating a yaw moment to the left, so that the vehicle heading is corrected to the left.

[0137] Unlike the direct and rapid intervention in current rapid correction, predictive correction adopts a gradual adjustment approach, gently adjusting the vehicle's course within the predicted duration (the first preset duration) to allow the vehicle to smoothly return to the target trajectory.

[0138] For example, consider a vehicle traveling north. If the predicted heading deviation is positive, it indicates that the predicted trajectory will deviate from the target trajectory, with the vehicle's predicted heading deviating to the left of north. The controller can then gradually increase the speed of the left drive unit and gradually decrease the speed of the right drive unit within the predicted time, based on the third power compensation amount, generating a yaw moment to the right, thus correcting the vehicle's heading to the right and returning it to the target north heading. If the predicted heading deviation is negative, it indicates that the predicted trajectory will exceed the target trajectory, with the vehicle's predicted heading deviating to the right of north. The controller can then gradually increase the speed of the right drive unit and gradually decrease the speed of the left drive unit within the predicted time, based on the third power compensation amount, generating a yaw moment to the left, thus correcting the vehicle's heading to the left until it reaches the target north heading.

[0139] In this embodiment, when the controller determines that the current heading deviation is small, it can further evaluate and predict the heading deviation. When the predicted deviation exceeds a threshold, it calculates the compensation amount using a PID control algorithm and corrects the deviation using a correction coefficient determined by the current heading deviation. Ultimately, the controller gradually and smoothly adjusts the heading by controlling the drive unit. In this process, the controller achieves prediction and early compensation for future trajectory deviation trends through quantitative evaluation of the predicted heading deviation, precise calculation using the PID control algorithm, and dynamic adjustment of the correction coefficient. This effectively synergizes with the current rapid correction strategy, allowing for gradual compensation for potential future trajectory deviations while ensuring the stability of the current heading. This comprehensively improves the trajectory tracking accuracy, navigation smoothness, and safety of vehicles navigating in complex water environments.

[0140] Figure 5 This is a flowchart illustrating Embodiment 4 of the trajectory control method for underwater vehicles provided in this application. Please refer to... Figure 5 In addition to any of the above embodiments, the method further includes:

[0141] S501. Calculate the current target heading angle and the predicted target heading angle based on the vehicle's wheelbase, distance from the center of gravity to the rear axle, front wheel steering angle, and vehicle speed.

[0142] In this step, the controller can acquire the vehicle's wheelbase, distance from the center of gravity to the rear axle, front wheel steering angle, and current vehicle speed. The wheelbase and distance from the center of gravity to the rear axle are inherent vehicle parameters and can be pre-stored in the controller; the front wheel steering angle can be acquired via a steering wheel angle sensor; and the vehicle speed can be acquired via a vehicle speed sensor. Furthermore, the controller can calculate the current target heading angle and the predicted target heading angle based on these parameters.

[0143] In practice, the controller calculates the sideslip angle based on the vehicle's wheelbase, distance from the center of gravity to the rear axle, front wheel steering angle, and a sideslip angle correction coefficient. Simultaneously, it calculates the current instantaneous yaw rate based on the vehicle's wheelbase, speed, front wheel steering angle, and yaw rate correction coefficient, and determines the current target yaw angle by multiplying the current instantaneous yaw rate by the sampling time. Then, it calculates the instantaneous average yaw rate based on the current instantaneous yaw rate and multiple historical instantaneous yaw rates within a second preset time period prior to the current moment, and determines the predicted target yaw angle by multiplying the instantaneous average yaw rate by a first preset time period. Finally, the sum of the sideslip angle and the current target yaw angle is determined as the current target heading angle, and the sum of the sideslip angle and the predicted target yaw angle is determined as the predicted target heading angle.

[0144] The sideslip angle correction coefficient is a parameter pre-calibrated through actual vehicle testing. It is used to correct the deviation of the sideslip angle caused by water resistance and propeller thrust characteristics when the vehicle is navigating in water, making the calculated sideslip angle more consistent with the actual motion state of the vehicle. The yaw rate correction coefficient is also pre-calibrated through actual vehicle testing. It is used to correct the influence of propulsion system power response delay and water flow disturbance on the yaw rate, thereby improving the calculation accuracy of the target yaw rate.

[0145] The specific values ​​of the two correction coefficients mentioned above are optimized by minimizing the deviation between the actual heading angle and the target heading angle. These values ​​were obtained through calibration via real vehicle testing, and the optimal values ​​can be determined using the least squares method or a genetic algorithm. Experimental verification shows that the typical range for the center-of-gravity sideslip angle correction coefficient is 0.6–1.2, and the typical range for the yaw rate correction coefficient is 0.7–1.3. The actual values ​​can be adaptively adjusted according to the vehicle's speed and load conditions.

[0146] The sampling time refers to the time interval between two consecutive samples taken by the controller, which can be set according to actual control requirements, for example, from 10 milliseconds to 100 milliseconds. In the embodiments of this application, the sampling time is used to convert the instantaneous yaw rate into the change in yaw rate within a sampling period, that is, by multiplying the instantaneous yaw rate by the sampling time, the target yaw rate within the sampling period is obtained.

[0147] The first preset duration represents the prediction time length, i.e., the time span for predicting the future course; the second preset duration represents the length of the historical data collection time window, i.e., the past time period used to calculate the historical average. It should be noted that the first and second preset durations can be the same or different, and can be calibrated based on the results of actual vehicle tests.

[0148] S502. Determine the current actual heading angle based on the GPS signal or the current yaw rate.

[0149] In this step, the controller can determine the current actual heading angle based on the GPS signal or the current yaw rate collected by the yaw rate sensor.

[0150] In practice, the controller can determine whether the current confidence level of the GPS signal is greater than a preset confidence threshold. If the current confidence level is greater than the preset confidence threshold, the current heading angle output by the GPS is determined as the current actual heading angle; if the current confidence level is less than or equal to the preset confidence threshold, the current actual heading angle is calculated based on the centroid sideslip angle and the current yaw rate.

[0151] The current confidence level of the Global Positioning System (GPS) signal characterizes the reliability of the GPS output signal, and its value is affected by factors such as satellite signal strength, reception quality, and the number of visible satellites. A pre-set confidence threshold can be calibrated through vehicle testing to determine the reliability of the current GPS signal. When the confidence level is higher than this threshold, the GPS output heading angle is considered accurate and reliable and can be directly used; when the confidence level is lower than or equal to this threshold, the GPS signal is considered unreliable, and the heading angle calculation must be switched to sensor fusion mode.

[0152] The current yaw rate is acquired in real time by the yaw rate sensor and is used to characterize the angular velocity of the vehicle rotating around the vertical axis at the current moment.

[0153] S503. Calculate and predict the actual heading angle based on the current yaw rate and multiple historical yaw rates within the second preset time period before the current moment.

[0154] In this step, the controller can obtain the current yaw rate and multiple historical yaw rates within a second preset time period before the current moment, and then calculate and predict the actual heading angle based on the above parameters.

[0155] In practice, the controller can calculate the average yaw rate by integrating the current yaw rate and multiple historical yaw rates; the product of the average yaw rate and the first preset time is determined as the predicted heading angle change; and the sum of the centroid sideslip angle and the predicted heading angle change is determined as the predicted actual heading angle.

[0156] Among them, the historical yaw rate is the yaw rate value recorded at each sampling time before the current time, and multiple historical yaw rates constitute the time series data used for prediction.

[0157] In this embodiment, the controller can calculate the current target heading angle and the predicted target heading angle based on the vehicle's wheelbase, distance from the center of gravity to the rear axle, front wheel steering angle, and vehicle speed; determine the current actual heading angle based on the GPS signal or the current yaw rate; and calculate the predicted actual heading angle based on the current yaw rate and historical yaw rates. This process integrates vehicle kinematic parameters, real-time sensor signals, and historical data, providing a reliable data foundation for the quantitative assessment of heading deviation and trajectory correction, thereby effectively improving the accuracy of trajectory control and navigation safety for vehicles navigating in water.

[0158] Figure 6 This is a flowchart illustrating Embodiment 5 of the trajectory control method for underwater vehicles provided in this application. Please refer to... Figure 6 Based on the above embodiment four, the specific implementation process of step S501 is as follows:

[0159] S601. Calculate the center of gravity sideslip angle based on the vehicle's wheelbase, distance from the center of gravity to the rear axle, front wheel steering angle, and center of gravity sideslip angle correction coefficient.

[0160] In this step, the controller can determine the ratio of the distance from the center of gravity to the rear axle to the wheelbase as the center of gravity wheelbase ratio, and then determine the center of gravity sideslip angle by multiplying the front wheel steering angle, the center of gravity wheelbase ratio, and the center of gravity sideslip angle correction coefficient.

[0161] S602. Calculate the current instantaneous yaw rate based on the vehicle's wheelbase, speed, front wheel angle, and yaw rate correction coefficient.

[0162] In this step, the controller can determine the ratio of vehicle speed to wheelbase as the vehicle speed-wheelbase ratio, and then determine the current instantaneous yaw rate by multiplying the front wheel steering angle, the vehicle speed-wheelbase ratio, and the yaw rate correction coefficient.

[0163] S603. The product of the current instantaneous yaw rate and the sampling time is determined as the current target yaw angle.

[0164] S604. Calculate the predicted target yaw angle based on the current instantaneous yaw rate and multiple historical instantaneous yaw rates within a second preset time period prior to the current moment.

[0165] In this step, the controller can calculate the integral average of the current instantaneous yaw rate and multiple historical instantaneous yaw rates to obtain the instantaneous average yaw rate. Then, the product of the instantaneous average yaw rate and the first preset duration is determined as the predicted target yaw angle.

[0166] Specifically, instantaneous average yaw rate It can be calculated using the following formula (2):

[0167] Formula (2)

[0168] in, This represents the instantaneous yaw rate function that varies with time. This indicates the time since the nth sampling moment in the past. up to the current sampling time The integral, i.e., the total cumulative yaw rate over that time period; the current instantaneous yaw rate. That is exist The value at time, i.e. The integration interval includes the yaw rate values ​​at all past sampling times, as well as the instantaneous yaw rate at the current time. n represents the number of sampling periods. This represents the total duration of the integration interval (i.e., the second preset duration).

[0169] S605. The sum of the sideslip angle of the center of gravity and the current target yaw angle is determined as the current target heading angle, and the sum of the sideslip angle of the center of gravity and the predicted target yaw angle is determined as the predicted target heading angle.

[0170] In this step, the summation operation can be performed on the sideslip angle of the center of mass and the current target yaw angle, and on the sideslip angle of the center of mass and the predicted target yaw angle, respectively. The two summation results are used as the current target heading angle and the predicted target heading angle, respectively.

[0171] In this embodiment, the controller can calculate the center-of-gravity sideslip angle, current instantaneous yaw rate, current target yaw angle, and predicted target yaw angle sequentially based on kinematic parameters such as vehicle wheelbase, distance from center of gravity to rear axle, front wheel steering angle, and vehicle speed, combined with the center-of-gravity sideslip angle correction coefficient and yaw rate correction coefficient. This ultimately yields the current target heading angle and the predicted target heading angle. This calculation process organically combines vehicle structural parameters, real-time sensing information, and historical operational data, enabling precise determination of the current desired heading and prediction of future heading trends. This provides solid data support for deviation calculation and trajectory control, ultimately improving the accuracy of heading control and driving safety of vehicles navigating in water.

[0172] Figure 7 This is a flowchart illustrating Embodiment Six of the trajectory control method for underwater vehicles provided in this application. Please refer to... Figure 7 Based on the above embodiment five, the specific implementation process of step S502 is as follows:

[0173] S701. Determine whether the current confidence level of the Global Positioning System signal is greater than the preset confidence threshold.

[0174] In this step, the controller can obtain the current confidence level of the GPS signal and compare it with a pre-calibrated preset confidence threshold to determine whether the current confidence level is greater than the preset confidence threshold.

[0175] Alternatively, the controller may acquire the current confidence level of the GPS signal in any of the following ways.

[0176] Method (1): The controller can directly read the confidence assessment value calculated and output by the global positioning system. This assessment value is in numerical form and is generated by the global positioning system based on factors such as satellite signal quality and positioning accuracy.

[0177] Method (2): The controller can calculate the current confidence level based on the signal strength output by the GPS module and the number of visible satellites. The higher the signal strength and the more visible satellites, the higher the confidence level; conversely, the lower the signal strength and the fewer visible satellites, the lower the confidence level.

[0178] S702. If the current confidence level is greater than the preset confidence threshold, the current heading angle output by the global positioning system shall be determined as the current actual heading angle.

[0179] In this step, when the controller determines that the current confidence level of the GPS signal is greater than the preset confidence threshold, it considers the heading angle output by the GPS to be accurate and reliable, and the controller can directly use the heading angle as the current actual heading angle without additional calculation.

[0180] S703. If the current confidence level is less than or equal to the preset confidence threshold, calculate the current actual heading angle based on the centroid sideslip angle and the current yaw rate.

[0181] In this step, when the controller determines that the current confidence level of the GPS signal is less than or equal to the preset confidence threshold, the product of the current yaw rate and the sampling time can be determined as the current heading angle change. Then, the sum of the centroid sideslip angle and the current heading angle change can be determined as the current actual heading angle.

[0182] In this embodiment, the controller can adaptively select the current actual heading angle determination method based on the confidence level of the Global Positioning System (GPS) signal. When the confidence level is high, the current heading angle output by the GPS is directly used as the current actual heading angle to ensure the accuracy of the heading information. When the confidence level is low, the controller switches to a calculation method based on sensor data, obtaining the current actual heading angle by fusing the centroid sideslip angle and the current yaw rate, ensuring reliable heading information can still be obtained in environments with missing or interfered signals. This mechanism enables the controller to adapt to signal conditions in different water environments, providing continuous and accurate current actual heading angles for subsequent trajectory correction, thereby improving the robustness and reliability of trajectory control for vehicles navigating in water.

[0183] Figure 8 This is a flowchart illustrating Embodiment Seven of the trajectory control method for underwater vehicles provided in this application. Please refer to... Figure 8 Based on the above embodiment five, the specific implementation process of step S503 is as follows:

[0184] S801. Calculate the integral average of the current yaw rate and multiple historical yaw rates to obtain the average yaw rate.

[0185] In this step, the controller can acquire the current yaw rate and multiple historical yaw rates within a second preset time period before the current moment, and calculate the integral average of these yaw rates to obtain the average yaw rate.

[0186] Specifically, the average yaw rate can be calculated using the following formula (3):

[0187] Formula (3)

[0188] in, Indicates the average yaw rate. This represents the actual yaw rate function that varies with time. This indicates the time since the nth sampling moment in the past. up to the current sampling time The integral, i.e., the total cumulative amount of actual yaw rate during that time period; the current yaw rate. That is exist The value at time, i.e. The integration interval includes the actual yaw rate values ​​at all past sampling times, as well as the current yaw rate. .

[0189] S802. The product of the average yaw rate and the first preset duration is determined as the predicted change in heading angle.

[0190] S803. The sum of the change in the centroid sideslip angle and the predicted heading angle is determined as the predicted actual heading angle.

[0191] In this embodiment, the controller can calculate the average yaw rate based on the current yaw rate and historical data through integration and averaging, thereby predicting the change in heading angle over a future period and ultimately obtaining the predicted actual heading angle. This process integrates real-time sensor data and historical trends, enabling early prediction of vehicle heading deviations and providing a reliable basis for initiating predictive correction strategies. This allows for smooth compensation for future trajectory deviations, improving the vehicle's ability to maintain its trajectory and its ride comfort while navigating in water.

[0192] Figure 9 This is a schematic diagram of the trajectory control device for an underwater vehicle provided in an embodiment of this application. Please refer to [link / reference]. Figure 9 The trajectory control device 10 for underwater vehicles includes:

[0193] The first processing module 11 is used to calculate the heading deviation based on the actual heading angle and the target heading angle when the vehicle is navigating in water. The target heading angle is calculated based on the vehicle's wheelbase, the distance from the center of gravity to the rear axle, the front wheel steering angle, and the vehicle speed.

[0194] The second processing module 12 is used to control the vehicle's drive system based on the heading deviation to correct the vehicle's trajectory.

[0195] The trajectory control device for underwater vehicles provided in this application embodiment can execute the technical solution shown in the above method embodiment. Its implementation principle and beneficial effects are similar, and will not be described again here.

[0196] In one possible implementation, the heading deviation includes a current heading deviation calculated based on the current actual heading angle and the current target heading angle; the second processing module 12 is specifically used for:

[0197] If the absolute value of the current heading deviation is greater than the first preset threshold, then the first power compensation amount is determined based on the current heading deviation and the current target heading angle.

[0198] Based on the first power compensation amount, the vehicle's drive system is controlled to adjust the vehicle's heading.

[0199] In one possible implementation, the heading deviation also includes the predicted heading deviation at a predicted time after a first preset time period, the predicted heading deviation being calculated based on the predicted actual heading angle and the predicted target heading angle after the first preset time period; the second processing module 12 is specifically used for:

[0200] If the absolute value of the current heading deviation is less than or equal to the first preset threshold, and the absolute value of the predicted heading deviation is greater than the second preset threshold, the second power compensation amount is determined based on the predicted actual heading angle and the predicted target heading angle.

[0201] The second power compensation is corrected according to the power compensation correction coefficient to obtain the third power compensation. The power compensation correction coefficient is calculated based on the angle difference between the current target heading angle and the current actual heading angle.

[0202] The vehicle's heading is adjusted by controlling the vehicle's drive system based on the third power compensation amount.

[0203] In one possible implementation, the second processing module 12 is specifically used for:

[0204] The difference between the predicted target heading angle and the predicted actual heading angle is calculated to obtain the predicted heading angle deviation;

[0205] Based on the predicted heading angle deviation, the second power compensation amount is calculated using a PID control algorithm.

[0206] In one possible implementation, the second processing module 12 is specifically used for:

[0207] Calculate the angle difference between the current target heading angle and the current actual heading angle;

[0208] Substitute the angle difference into the preset correction coefficient calculation formula to obtain the compensation correction coefficient.

[0209] In one possible implementation, the second processing module 12 is specifically used for:

[0210] Obtain a preset compensation table, which includes the power compensation amount corresponding to different heading deviations and different target heading angles;

[0211] Based on the current course deviation and the current target course angle, consult the compensation table to obtain the first power compensation amount.

[0212] In one possible implementation, the second processing module 12 is specifically used for:

[0213] Determine the left-hand drive unit and the right-hand drive unit based on the vehicle's direction of travel;

[0214] If the current heading deviation is positive, the power output of the left drive unit is increased and the power output of the right drive unit is decreased according to the first power compensation amount.

[0215] If the current heading deviation is negative, the power output of the right drive unit is increased and the power output of the left drive unit is decreased according to the first power compensation amount.

[0216] In one possible implementation, the second processing module 12 is specifically used for:

[0217] If the predicted heading deviation is positive, the power output of the left drive unit is increased and the power output of the right drive unit is decreased according to the third power compensation amount. The left and right drive units are determined according to the vehicle's forward direction.

[0218] If the predicted heading deviation is negative, the power output of the right drive unit will be increased and the power output of the left drive unit will be decreased according to the third power compensation amount.

[0219] In one possible implementation, the first processing module 11 is further configured to:

[0220] Calculate the current target heading angle and the predicted target heading angle based on the vehicle's wheelbase, distance from the center of gravity to the rear axle, front wheel steering angle, and vehicle speed.

[0221] Determine the current actual heading angle based on the GPS signal or the current yaw rate;

[0222] The predicted heading angle is calculated based on the current yaw rate and multiple historical yaw rates within a second preset time period prior to the current moment.

[0223] In one possible implementation, the first processing module 11 is specifically used for:

[0224] The center of gravity sideslip angle is calculated based on the vehicle's wheelbase, distance from the center of gravity to the rear axle, front wheel steering angle, and center of gravity sideslip angle correction coefficient.

[0225] Calculate the current instantaneous yaw rate based on the vehicle's wheelbase, speed, front wheel steering angle, and yaw rate correction factor;

[0226] The product of the current instantaneous yaw rate and the sampling time is determined as the current target yaw angle;

[0227] The predicted target yaw angle is calculated based on the current instantaneous yaw rate and multiple historical instantaneous yaw rates within a second preset time period prior to the current moment;

[0228] The sum of the center of gravity sideslip angle and the current target yaw angle is determined as the current target heading angle, and the sum of the center of gravity sideslip angle and the predicted target yaw angle is determined as the predicted target heading angle.

[0229] In one possible implementation, the first processing module 11 is specifically used for:

[0230] The ratio of the distance from the center of mass to the rear axle to the wheelbase is defined as the center of mass-wheelbase ratio.

[0231] The product of the front wheel steering angle, the wheelbase-to-center-of-gravity ratio, and the correction factor for the center-of-gravity sideslip angle is determined as the center-of-gravity sideslip angle.

[0232] In one possible implementation, the first processing module 11 is specifically used for:

[0233] The ratio of vehicle speed to wheelbase is defined as the speed-wheelbase ratio.

[0234] The product of the front wheel steering angle, the vehicle speed-to-wheelbase ratio, and the yaw rate correction factor is determined as the current instantaneous yaw rate.

[0235] In one possible implementation, the first processing module 11 is specifically used for:

[0236] Calculate the integral average of the current instantaneous yaw rate and multiple historical instantaneous yaw rates to obtain the instantaneous average yaw rate;

[0237] The product of the instantaneous average yaw rate and the first preset duration is determined as the predicted target yaw angle.

[0238] In one possible implementation, the first processing module 11 is specifically used for:

[0239] Determine whether the current confidence level of the Global Positioning System (GPS) signal is greater than a preset confidence threshold;

[0240] If the current confidence level is greater than the preset confidence threshold, the current heading angle output by the Global Positioning System will be determined as the current actual heading angle.

[0241] If the current confidence level is less than or equal to the preset confidence threshold, the current actual heading angle is calculated based on the centroid sideslip angle and the current yaw rate.

[0242] In one possible implementation, the first processing module 11 is specifically used for:

[0243] The product of the current yaw rate and the sampling time is determined as the change in the current heading angle;

[0244] The sum of the sideslip angle and the change in the current heading angle is determined as the current actual heading angle.

[0245] In one possible implementation, the first processing module 11 is specifically used for:

[0246] Calculate the integral average of the current yaw rate and multiple historical yaw rates to obtain the average yaw rate;

[0247] The product of the average yaw rate and the first preset time is determined as the predicted change in heading angle;

[0248] The sum of the change in the center of gravity sideslip angle and the predicted heading angle is determined as the predicted actual heading angle.

[0249] The trajectory control device for underwater vehicles provided in this application embodiment can execute the technical solution shown in the above method embodiment. Its implementation principle and beneficial effects are similar, and will not be described again here.

[0250] Figure 10 This is a schematic diagram of the controller provided in an embodiment of this application. Please refer to... Figure 10 The controller 20 provided in this embodiment can be a vehicle controller, an autonomous driving domain controller, a chassis domain controller deployed in a vehicle, or a specially set underwater navigation trajectory controller. This application does not limit this.

[0251] The controller 20 may include at least one processor 21 and a memory 22. Optionally, the controller 20 may also include a communication component 23. The processor 21, the memory 22, and the communication component 23 are connected via a bus 24.

[0252] In the specific implementation process, at least one processor 21 executes computer execution instructions stored in memory 22, causing at least one processor 21 to perform the above-described method.

[0253] The specific implementation process of processor 21 can be found in the above method embodiments, and its implementation principle and technical effect are similar. It will not be repeated here.

[0254] In the above embodiments, it should be understood that the processor can be a Central Processing Unit (CPU), or other general-purpose processors, digital signal processors (DSPs), application-specific integrated circuits (ASICs), etc. The general-purpose processor can be a microprocessor or any conventional processor. The steps of the method disclosed in this invention can be directly implemented by a hardware processor, or implemented by a combination of hardware and software modules within the processor.

[0255] The memory may include random access memory (RAM) and may also include non-volatile memory (NVM), such as at least one disk storage device.

[0256] The bus can be an Industry Standard Architecture (ISA) bus, a Peripheral Component Interconnect (PCI) bus, or an Extended Industry Standard Architecture (EISA) bus, etc. Buses can be categorized as address buses, data buses, control buses, etc. For ease of illustration, the buses shown in the accompanying drawings are not limited to a single bus or a single type of bus.

[0257] This application provides a vehicle, including a vehicle body and... Figure 10 The controller shown is used to implement the trajectory control method for underwater vehicles provided in the above-described method embodiments.

[0258] This application also provides a computer program product, including a computer program that, when executed by a processor, implements the above-described method.

[0259] This application also provides a computer-readable storage medium storing computer-executable instructions, which, when executed by a processor, implement the above-described method.

[0260] The aforementioned readable storage medium can be implemented by any type of volatile or non-volatile storage device or a combination thereof, such as static random access memory (SRAM), electrically erasable programmable read-only memory (EEPROM), erasable programmable read-only memory (EPROM), programmable read-only memory (PROM), read-only memory (ROM), magnetic storage, flash memory, magnetic disk, or optical disk. The readable storage medium can be any available medium accessible to a general-purpose or special-purpose computer.

[0261] An exemplary readable storage medium is coupled to a processor, enabling the processor to read information from and write information to the readable storage medium. Of course, the readable storage medium can also be a component of the processor. The processor and the readable storage medium can reside in an Application Specific Integrated Circuit (ASIC). Alternatively, the processor and the readable storage medium can exist as discrete components in the device.

[0262] The division of units is merely a logical functional division; in actual implementation, there may be other division methods. For example, multiple units or components may be combined or integrated into another system, or some features may be ignored or not executed. Furthermore, the coupling or direct coupling or communication connection shown or discussed may be indirect coupling or communication connection through some interfaces, devices, or units, and may be electrical, mechanical, or other forms.

[0263] The units described as separate components may or may not be physically separate. The components shown as units may or may not be physical units; that is, they may be located in one place or distributed across multiple network units. Some or all of the units can be selected to achieve the purpose of this embodiment according to actual needs.

[0264] In addition, the functional units in the various embodiments of the present invention can be integrated into one processing unit, or each unit can exist physically separately, or two or more units can be integrated into one unit.

[0265] If a function is implemented as a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of this invention, or the part that contributes to the prior art, or a part of the technical solution, can be embodied in the form of a software product. This computer software product is stored in a storage medium and includes several instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) to execute all or part of the steps of the methods of the various embodiments of this invention. The aforementioned storage medium includes various media capable of storing program code, such as USB flash drives, portable hard drives, read-only memory (ROM), random access memory (RAM), magnetic disks, or optical disks.

[0266] Those skilled in the art will understand that all or part of the steps of the above-described method embodiments can be implemented by hardware related to program instructions. The aforementioned program can be stored in a computer-readable storage medium. When executed, the program performs the steps of the above-described method embodiments; and the aforementioned storage medium includes various media capable of storing program code, such as ROM, RAM, magnetic disks, or optical disks.

[0267] Finally, it should be noted that other embodiments of the invention will readily occur to those skilled in the art upon consideration of the specification and practice of the invention disclosed herein. This invention is intended to cover any variations, uses, or adaptations of the invention that follow the general principles of the invention and include common knowledge or customary techniques in the art not disclosed herein, and is not limited to the precise structures described above and shown in the accompanying drawings, and various modifications and changes can be made without departing from its scope.

Claims

1. A trajectory control method for a vehicle navigating in water, characterized in that, include: When a vehicle is navigating in water, the heading deviation is calculated based on the actual heading angle and the target heading angle. The target heading angle is calculated based on the vehicle's wheelbase, the distance from the center of gravity to the rear axle, the front wheel steering angle, and the vehicle speed. The heading deviation includes the current heading deviation and the predicted heading deviation at a predicted time after a first preset time. Based on the heading deviation, the vehicle's drive system is controlled to correct the vehicle's trajectory. The method of controlling the vehicle's drive system based on the heading deviation includes: If the absolute value of the current heading deviation is less than or equal to a first preset threshold, and the absolute value of the predicted heading deviation is greater than a second preset threshold, a second power compensation amount is determined based on the predicted actual heading angle and the predicted target heading angle; the second power compensation amount is corrected based on the power compensation amount correction coefficient to obtain a third power compensation amount, wherein the power compensation amount correction coefficient is calculated based on the angle difference between the current target heading angle and the current actual heading angle; Based on the third power compensation amount, the vehicle's drive unit is controlled to adjust the vehicle's heading.

2. The method according to claim 1, characterized in that, The current heading deviation is calculated based on the current actual heading angle and the current target heading angle; The method of controlling the vehicle's drive system based on the heading deviation includes: If the absolute value of the current heading deviation is greater than the first preset threshold, then the first power compensation amount is determined based on the current heading deviation and the current target heading angle. Based on the first power compensation amount, the vehicle's drive unit is controlled to adjust the vehicle's heading.

3. The method according to claim 2, characterized in that, The predicted heading deviation is calculated based on the predicted actual heading angle and the predicted target heading angle for the first preset time period.

4. The method according to claim 3, characterized in that, The step of determining the second power compensation amount based on the predicted actual heading angle and the predicted target heading angle includes: The difference between the predicted target heading angle and the predicted actual heading angle is calculated to obtain the predicted heading angle deviation; Based on the predicted heading angle deviation, the second power compensation amount is calculated using a PID control algorithm.

5. The method according to claim 3, characterized in that, The method further includes: Calculate the angle difference between the current target heading angle and the current actual heading angle; Substituting the angle difference into the preset correction coefficient calculation formula, the compensation amount correction coefficient is obtained.

6. The method according to claim 2, characterized in that, The step of determining the first power compensation amount based on the current heading deviation and the current target heading angle includes: Obtain a preset compensation table, which includes power compensation amounts corresponding to different heading deviations and different target heading angles; Based on the current heading deviation and the current target heading angle, the compensation table is consulted to obtain the first power compensation amount.

7. The method according to claim 6, characterized in that, The step of controlling the vehicle's drive unit to adjust the vehicle's heading based on the first power compensation amount includes: Based on the vehicle's direction of travel, determine the left-side drive unit and the right-side drive unit; If the current heading deviation is positive, the power output of the left drive unit is increased and the power output of the right drive unit is decreased according to the first power compensation amount. If the current heading deviation is negative, the power output of the right drive unit is increased and the power output of the left drive unit is decreased according to the first power compensation amount.

8. The method according to claim 3, characterized in that, The step of controlling the vehicle's drive unit to adjust the vehicle's heading based on the third power compensation amount includes: If the predicted heading deviation is positive, the power output of the left drive unit is increased and the power output of the right drive unit is decreased according to the third power compensation amount. The left and right drive units are determined according to the vehicle's forward direction. If the predicted heading deviation is negative, the power output of the right drive unit is increased and the power output of the left drive unit is decreased according to the third power compensation amount.

9. The method according to any one of claims 1 to 8, characterized in that, The method further includes: Calculate the current target heading angle and the predicted target heading angle based on the vehicle's wheelbase, the distance from the center of gravity to the rear axle, the front wheel steering angle, and the vehicle speed. Determine the current actual heading angle based on the GPS signal or the current yaw rate; The predicted heading angle is calculated based on the current yaw rate and multiple historical yaw rates within a second preset time period prior to the current moment.

10. The method according to claim 9, characterized in that, The calculation of the current target heading angle and the predicted target heading angle based on the vehicle's wheelbase, the distance from the center of gravity to the rear axle, the front wheel steering angle, and the vehicle speed includes: The center of gravity sideslip angle is calculated based on the vehicle's wheelbase, the distance from the center of gravity to the rear axle, the front wheel steering angle, and the center of gravity sideslip angle correction coefficient. The current instantaneous yaw rate is calculated based on the vehicle's wheelbase, vehicle speed, front wheel steering angle, and yaw rate correction coefficient. The product of the current instantaneous yaw rate and the sampling time is determined as the current target yaw angle; The predicted target yaw angle is calculated based on the current instantaneous yaw rate and multiple historical instantaneous yaw rates within a second preset time period prior to the current moment; The sum of the centroid sideslip angle and the current target yaw angle is determined as the current target heading angle, and the sum of the centroid sideslip angle and the predicted target yaw angle is determined as the predicted target heading angle.

11. The method according to claim 10, characterized in that, The calculation of the center-of-gravity sideslip angle based on the vehicle's wheelbase, the distance from the center of gravity to the rear axle, the front wheel steering angle, and the center-of-gravity sideslip angle correction coefficient includes: The ratio of the distance from the center of mass to the rear axle to the wheelbase is defined as the center of mass-wheelbase ratio. The product of the front wheel steering angle, the center of gravity wheelbase ratio, and the center of gravity sideslip angle correction coefficient is determined as the center of gravity sideslip angle.

12. The method according to claim 10, characterized in that, The step of calculating the current instantaneous yaw rate based on the vehicle's wheelbase, vehicle speed, front wheel steering angle, and yaw rate correction coefficient includes: The ratio of the vehicle speed to the wheelbase is defined as the speed-wheelbase ratio. The product of the front wheel steering angle, the vehicle speed-to-wheelbase ratio, and the yaw rate correction coefficient is determined as the current instantaneous yaw rate.

13. The method according to claim 10, characterized in that, The step of calculating the predicted target yaw angle based on the current instantaneous yaw rate and multiple historical instantaneous yaw rates within a second preset time period prior to the current moment includes: Calculate the integral average of the current instantaneous yaw rate and the multiple historical instantaneous yaw rates to obtain the instantaneous average yaw rate; The product of the instantaneous average yaw rate and the first preset duration is determined as the predicted target yaw angle.

14. The method according to claim 10, characterized in that, Determining the current actual heading angle based on the Global Positioning System signal or the current yaw rate includes: Determine whether the current confidence level of the Global Positioning System signal is greater than a preset confidence threshold; If the current confidence level is greater than the preset confidence threshold, then the current heading angle output by the global positioning system is determined as the current actual heading angle; If the current confidence level is less than or equal to the preset confidence threshold, then the current actual heading angle is calculated based on the centroid sideslip angle and the current yaw rate.

15. The method according to claim 14, characterized in that, The step of calculating the current actual heading angle based on the sideslip angle and the current yaw rate includes: The product of the current yaw rate and the sampling time is determined as the change in the current heading angle; The sum of the change in the centroid sideslip angle and the change in the current heading angle is determined as the current actual heading angle.

16. The method according to claim 10, characterized in that, The step of calculating and predicting the actual heading angle based on the current yaw rate and multiple historical yaw rates within a second preset time period prior to the current moment includes: Calculate the integral average of the current yaw rate and the multiple historical yaw rates to obtain the average yaw rate; The product of the average yaw rate and the first preset time is determined as the predicted change in heading angle; The sum of the change in the centroid sideslip angle and the predicted heading angle is determined as the predicted actual heading angle.

17. A trajectory control device for a vehicle navigating in water, characterized in that, include: The first processing module is used to calculate the heading deviation based on the actual heading angle and the target heading angle when the vehicle is navigating in water. The target heading angle is calculated based on the vehicle's wheelbase, the distance from the center of gravity to the rear axle, the front wheel steering angle, and the vehicle speed. The heading deviation includes the current heading deviation and the predicted heading deviation at a predicted time after a first preset time. The second processing module is used to control the vehicle's drive device based on the heading deviation to correct the vehicle's trajectory. The second processing module, when controlling the vehicle's drive system based on the heading deviation, is specifically used for: If the absolute value of the current heading deviation is less than or equal to a first preset threshold, and the absolute value of the predicted heading deviation is greater than a second preset threshold, a second power compensation amount is determined based on the predicted actual heading angle and the predicted target heading angle; the second power compensation amount is corrected based on the power compensation amount correction coefficient to obtain a third power compensation amount, wherein the power compensation amount correction coefficient is calculated based on the angle difference between the current target heading angle and the current actual heading angle; Based on the third power compensation amount, the vehicle's drive unit is controlled to adjust the vehicle's heading.

18. A controller, characterized in that, include: Memory, processor; The memory stores computer-executed instructions; The processor executes computer execution instructions stored in the memory, causing the processor to perform the method as described in any one of claims 1-16.

19. A vehicle, characterized in that, include: The vehicle body, and the controller as described in claim 18.

20. A computer-readable storage medium, characterized in that, The computer-readable storage medium stores computer-executable instructions, which, when executed by a processor, are used to implement the method as described in any one of claims 1-16.

Images