Electric camping car automatic following control method and system based on ultrasonic waves

Abstract

The invention provides an electric camping car automatic following control method and system based on ultrasonic waves, and relates to the technical field of electric car control, and the method comprises the steps: collecting target position information through an ultrasonic sensor array, determining and smoothing a target track, dynamically adjusting a filtering gain to construct a motion track equation, and generating a following control instruction; the method comprises the following steps: acquiring environment information to judge obstacles, distributing passage cost values to environment division grid units, constructing a path planning tree, selecting an optimal obstacle avoidance path, and correcting a control instruction to realize target following.

Description

Technical Field

[0001] This invention relates to the field of electric vehicle control technology, and in particular to an ultrasonic-based automatic following control method and system for electric camping vehicles. Background Technology

[0002] Electric camping vehicles, as a portable tool for outdoor living and travel, are gradually gaining popularity. In practical applications, users often need to carry a lot of equipment when moving in complex wilderness environments. If electric camping vehicles can automatically follow users, it will greatly improve the convenience of outdoor activities. Ultrasonic following technology, as a following method that does not require wearing specific markers, has the advantages of simple implementation and low cost, and has been widely used in various smart mobile devices.

[0003] Currently, the automatic following systems for electric camping vehicles on the market mainly rely on technologies such as visual recognition, radar detection, or wireless signal positioning to achieve target following. However, in practical applications, traditional automatic following technology still cannot effectively cope with the rapidly changing motion state of the target, resulting in an uneven trajectory during the following process. In particular, when the target accelerates or decelerates, it is easy to experience following delays or overshoots. It also lacks the ability to dynamically adjust filtering parameters, cannot adaptively optimize the following effect according to different scenarios, has low following accuracy in complex environments, and usually adopts fixed obstacle avoidance strategies, lacking the ability to comprehensively assess the environment. When facing environments with multiple obstacles or dynamic obstacles, it is easy to cause unreasonable obstacle avoidance paths or oscillating behavior. Therefore, a solution is urgently needed to address the problems existing in the current technology. Summary of the Invention

[0004] This invention provides an ultrasonic-based automatic following control method and system for electric camping vehicles, which can at least solve some of the problems existing in the prior art.

[0005] A first aspect of this invention provides an ultrasonic-based automatic following control method for electric camping vehicles, comprising: The spatial location information of the target object is collected by the ultrasonic sensor array of the electric camping vehicle to determine the real-time coordinate information and motion status information of the target object; The target trajectory is determined based on the real-time coordinate information and motion state information of the target object. The target trajectory is smoothed. The filter gain is adjusted by dynamically calculating the state noise covariance matrix and the measurement noise covariance matrix. The continuous motion trajectory equation of the target object is constructed and the target acceleration characteristics are calculated and obtained. The following control command of the electric camping vehicle is generated based on the acceleration characteristics. The follow control command is sent to the power system and steering system of the electric campervan; The system acquires environmental information during the electric camper's journey and determines whether there are obstacles. If there are obstacles, the current environment is divided into grid cells, and a passage cost is assigned to each grid cell. A path planning tree is constructed, a comprehensive score for each path is determined, and the optimal obstacle avoidance path is selected based on the comprehensive score. The following control command is then modified. The electric campervan's power and steering systems are controlled according to the revised follow control commands to follow the target.

[0006] In one alternative implementation, The spatial location information of the target object is collected by the ultrasonic sensor array of the electric camping vehicle, and the real-time coordinate information and motion status information of the target object are determined, including: The ultrasonic sensor array at the front of the electric campervan emits ultrasonic signals and receives reflected echo signals. The ultrasonic sensor array includes multiple ultrasonic sensors that are evenly arranged laterally along the front of the electric campervan. The distance between the target object and each ultrasonic sensor is calculated based on the received reflected echo signal, and the distance value is converted into spatial position information in a coordinate system with the electric camper vehicle as the origin. Based on the spatial location information, the real-time coordinates and motion status information of the target object relative to the electric camper are determined. The motion status information includes the target object's speed and direction of motion.

[0007] In one alternative implementation, Based on the real-time coordinate information and motion state information of the target object, the target trajectory is determined, and the target trajectory is smoothed. The filter gain is adjusted by dynamically calculating the state noise covariance matrix and the measurement noise covariance matrix. The continuous motion trajectory equation of the target object is constructed, and the target acceleration characteristics are calculated and obtained. The following control commands for the electric camping vehicle are generated based on the acceleration characteristics, including: The real-time coordinate information and motion state information of the target object are used to construct a target state vector. The target state vector is multiplied by the state transition matrix and state noise is added to obtain the state transition equation. The target trajectory is obtained using the state transition equation. The target trajectory is added to the state transition equation to determine the predicted state. The difference between the predicted state at adjacent time points is calculated to obtain the state increment. The difference between the measured value of the target trajectory and the predicted state is calculated to obtain the information sequence. The rate of change of the target trajectory within three consecutive frames is obtained. A first forgetting factor and a second forgetting factor are set based on the rate of change. The state noise covariance matrix is ​​dynamically calculated by iteratively calculating the state increment and the first forgetting factor. The measurement noise covariance matrix is ​​dynamically calculated by iteratively calculating the innovation sequence and the second forgetting factor. The filter gain is adjusted according to the state noise covariance matrix and the measurement noise covariance matrix. The filter gain is multiplied by the innovation sequence to smooth the target trajectory and obtain a smoothed target trajectory. The target trajectory after smoothing filtering is smoothed by cubic interpolation to obtain a continuous trajectory. The second difference of the continuous trajectory is calculated to obtain the target acceleration characteristics. The following control command of the electric camping vehicle is generated based on the target acceleration characteristics.

[0008] In one alternative implementation, The target trajectory after smoothing filtering is smoothed by cubic interpolation to obtain a continuous trajectory. The second-order difference of the continuous trajectory is calculated to obtain the target acceleration characteristics. Based on the target acceleration characteristics, the following follow control command for the electric campervan is generated, including: The velocity vector and direction angle are calculated based on the coordinate difference between adjacent trajectory points in the target trajectory after smoothing and filtering. The velocity vector is calculated by the ratio of the Euclidean distance between adjacent trajectory points to the sampling time interval, and the direction angle is calculated by the arctangent of the coordinate increments of adjacent trajectory points. The tangential acceleration is obtained by calculating the time derivative of the velocity vector. The normal acceleration is obtained by dividing the square of the velocity vector by the radius of curvature of the target trajectory after smoothing and filtering. The 2-norm of the tangential acceleration and the normal acceleration are calculated and exponentially operated on. The compensation weight is obtained by multiplying them by the pre-set adaptive weight coefficient. The pre-set control point coefficients are dynamically adjusted using the compensation weights. The adjusted control point coefficients are then weighted and calculated with the cubic interpolation basis function. The smoothed target trajectory is then smoothed by cubic interpolation to obtain a continuous trajectory. The target acceleration features are obtained by performing a second-order difference operation on the continuous trajectory. The target acceleration features include acceleration components in the horizontal and vertical directions. The following control command for the electric campervan is generated based on the target acceleration features.

[0009] In one alternative implementation, The system acquires environmental information during the electric campervan's journey and determines whether obstacles exist. If obstacles exist, the current environment is divided into grid cells, and a passage cost is assigned to each grid cell. A path planning tree is constructed, a comprehensive score for each path is determined, and the optimal obstacle avoidance path is selected based on the comprehensive score. The following control commands are then modified, including: The system acquires environmental information during the operation of the electric campervan, calculates the number of data points within a preset distance radius around each data point in the environmental information to obtain the data point density, marks data points with a density greater than a preset density threshold as core points, calculates the spatial distance between each core point, classifies core points with a spatial distance less than a preset distance radius into the same category, compares the number of data points in each category with a preset number threshold to obtain the obstacle judgment result, and extracts the center position and size from the obstacle to form an obstacle feature vector based on the obstacle judgment result. The current environment is divided into grid cells. The distance from each grid cell to the nearest obstacle is calculated based on the obstacle feature vector to obtain the obstacle occupancy cost. The distance from each grid cell to the target point is calculated to obtain the distance cost. The obstacle occupancy cost and the distance cost are weighted and combined to obtain the passage cost of each grid cell. An extended tree is generated from the start and end points. Sampling points are randomly sampled in the current environment. The nearest node is found in the extended tree. The sampling points are used to build extended branches according to a preset step size to obtain multiple connectable paths. The cumulative value of the travel cost of each connectable path is calculated based on the travel cost of each grid cell. A comprehensive score is calculated by weighting the length of each connectable path, the cumulative value of the passage cost, and the curvature. The optimal obstacle avoidance path is selected based on the comprehensive score, and the following control command is modified according to the optimal obstacle avoidance path.

[0010] In one alternative implementation, Generate an extended tree from the start and end points, randomly sample points in the current environment, find the nearest node in the extended tree, and construct extended branches from the sample points according to a preset step size to obtain multiple connectable paths, including: In an environment with obstacles, an extended tree is generated from the starting point and the ending point respectively. A line of sight is established from the starting point to the ending point. The set of intersection points between the line of sight and the obstacles is calculated. At each intersection point, the channel geometric features are extracted. The channel geometric features include the channel width indicating the minimum distance between obstacles, the channel direction vector indicating the tangent vector of the channel centerline, and the channel center point indicating the center position of the narrowest point of the channel. Calculate the distance from the location in the environment to the nearest channel center point, divide the distance by the channel width and take the negative exponent to obtain the attenuation factor, multiply the attenuation factor by the difference between the channel direction vector and the location projection vector and normalize it to obtain the guiding vector of the location in the environment. Sampling points are randomly sampled in the current environment. The normalized distance from the sampling point to the nearest channel center is multiplied by the first weight coefficient. The cosine similarity between the sampling direction and the guiding vector is multiplied by the second weight coefficient. The two are added together to obtain the sampling point score. Valid sampling points that meet the preset score threshold are selected based on the sampling point score. The first weight coefficient and the second weight coefficient are preset and their sum is 1. Find the nearest node to the effective sampling point in each of the two extended trees, and build extended branches from the effective sampling point to the two nearest nodes according to a preset step size to obtain new nodes. Add the new nodes to the corresponding extended trees, and repeat the addition until the extended trees are built to obtain multiple connectable paths.

[0011] In one alternative implementation, The revised follow control commands control the electric campervan's power and steering systems to follow a target, including: Receive a modified follow control command, wherein the modified follow control command includes a speed value and a steering angle value; The electric campervan's power system is controlled to adjust its speed according to the speed value in the revised follow control command; The steering system of the electric campervan is controlled to adjust its steering according to the steering angle value in the modified follow control command, so as to enable the electric campervan to follow the target.

[0012] A second aspect of the present invention provides an ultrasonic-based automatic following control system for electric campervans, comprising: The first unit is used to collect the spatial location information of the target object through the ultrasonic sensor array of the electric camping vehicle, and to determine the real-time coordinate information and motion status information of the target object; The second unit is used to determine the target trajectory based on the real-time coordinate information and motion state information of the target object, smooth the target trajectory, adjust the filter gain by dynamically calculating the state noise covariance matrix and the measurement noise covariance matrix, construct the continuous motion trajectory equation of the target object and calculate and obtain the target acceleration characteristics, and generate the following control command of the electric camping vehicle based on the acceleration characteristics. The third unit is used to send the follow control command to the power system and steering system of the electric campervan; The fourth unit is used to acquire environmental information during the electric camper's journey and determine whether there are obstacles. If there are obstacles, the current environment is divided into grid cells and a passage cost is assigned to each grid cell. A path planning tree is constructed, the comprehensive score of each path is determined, and the optimal obstacle avoidance path is selected based on the comprehensive score. The following control command is then modified. The fifth unit is used to control the power system and steering system of the electric campervan to follow the target according to the revised follow control instructions.

[0013] A third aspect of the present invention provides an electronic device, comprising: A processor and a memory for storing processor-executable instructions, wherein the processor is configured to invoke instructions stored in the memory to perform the aforementioned method.

[0014] A fourth aspect of the present invention provides a computer-readable storage medium having stored thereon computer program instructions that, when executed by a processor, implement the aforementioned method.

[0015] In this invention, by dynamically adjusting the filter gain to construct a continuous motion trajectory equation, the motion characteristics of the target object can be accurately captured, improving the response speed and accuracy of the following system. This allows the electric campervan to smoothly follow the target object. By smoothing the target trajectory and generating control commands based on acceleration characteristics, the rapid acceleration, deceleration, and sharp turns of the electric campervan during the following process are reduced, improving ride comfort and driving safety, and extending the service life of vehicle components. Combined with environmental perception capabilities, when an obstacle is detected, the optimal obstacle avoidance path is automatically calculated using gridded environment modeling and path planning tree algorithms, realizing intelligent obstacle avoidance function. This enhances the adaptability and safety of the electric campervan in complex environments and effectively avoids collision risks. Attached Figure Description

[0016] Figure 1 This is a flowchart illustrating the ultrasonic-based automatic following control method for electric camping vehicles according to an embodiment of the present invention. Figure 2 This is a schematic diagram comparing the target direction recognition angle deviation in an embodiment of the present invention; Figure 3 This is a logic block diagram of obstacle avoidance path planning for an electric camping vehicle according to an embodiment of the present invention. Detailed Implementation

[0017] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0018] The technical solution of the present invention will be described in detail below with reference to 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.

[0019] Figure 1 This is a flowchart illustrating the ultrasonic-based automatic following control method for electric camping vehicles according to an embodiment of the present invention. Figure 1 As shown, the method includes: The spatial location information of the target object is collected by the ultrasonic sensor array of the electric camping vehicle to determine the real-time coordinate information and motion status information of the target object; The target trajectory is determined based on the real-time coordinate information and motion state information of the target object. The target trajectory is smoothed. The filter gain is adjusted by dynamically calculating the state noise covariance matrix and the measurement noise covariance matrix. The continuous motion trajectory equation of the target object is constructed and the target acceleration characteristics are calculated and obtained. The following control command of the electric camping vehicle is generated based on the acceleration characteristics. The follow control command is sent to the power system and steering system of the electric campervan; The system acquires environmental information during the electric camper's journey and determines whether there are obstacles. If there are obstacles, the current environment is divided into grid cells, and a passage cost is assigned to each grid cell. A path planning tree is constructed, a comprehensive score for each path is determined, and the optimal obstacle avoidance path is selected based on the comprehensive score. The following control command is then modified. The electric campervan's power and steering systems are controlled according to the revised follow control commands to follow the target.

[0020] In one alternative implementation, The spatial location information of the target object is collected by the ultrasonic sensor array of the electric camping vehicle, and the real-time coordinate information and motion status information of the target object are determined, including: The ultrasonic sensor array at the front of the electric campervan emits ultrasonic signals and receives reflected echo signals. The ultrasonic sensor array includes multiple ultrasonic sensors that are evenly arranged laterally along the front of the electric campervan. The distance between the target object and each ultrasonic sensor is calculated based on the received reflected echo signal, and the distance value is converted into spatial position information in a coordinate system with the electric camper vehicle as the origin. Based on the spatial location information, the real-time coordinates and motion status information of the target object relative to the electric camper are determined. The motion status information includes the target object's speed and direction of motion.

[0021] The electric campervan is equipped with an ultrasonic sensor array at the front, consisting of five ultrasonic sensors operating at a frequency of 40kHz. The measurement distance range is from 2cm to 400cm, with a measurement accuracy of ±3mm. The five ultrasonic sensors are evenly distributed laterally along the front of the campervan, with a distance of 30cm between adjacent sensors, covering a 120cm width area at the front of the campervan. Each ultrasonic sensor is fixed to a bracket 25cm below the front bumper, with the sensor's emitting surface parallel to the ground and the angle adjusted to 0 degrees to ensure that the emitted ultrasonic signal effectively covers the area in front of the campervan.

[0022] When the ultrasonic sensor array is working, the controller controls each sensor to operate sequentially according to a preset timing sequence to avoid mutual interference. Each sensor has a working cycle of 60ms, with 10μs spent transmitting the ultrasonic signal and a maximum of 59.99ms spent receiving the reflected echo signal. The controller sends a 10μs high-level pulse to the sensor's trigger pin. Upon receiving the trigger signal, the sensor automatically transmits eight 40kHz ultrasonic pulses. When the ultrasonic pulse encounters the target object and reflects back, the sensor's receiver detects the echo signal, and the output pin generates a high-level signal. By measuring the duration of the high-level signal, the round-trip time of the ultrasonic wave can be calculated.

[0023] Distance calculation uses the speed of sound in air (340 m / s) to calculate the distance between the target object and the sensor based on the round-trip time. For example, when a high-level signal is detected for a duration of 5882 μs, the calculated distance between the target object and the sensor is (340 × 5882 × 10). -6 ) / 2=1m. To improve measurement accuracy, data was collected five times consecutively for each sensor, and the maximum and minimum values ​​were removed before taking the average value as the final distance value.

[0024] The distance values ​​measured by each sensor are converted into spatial position information in the coordinate system of the electric campervan. A Cartesian coordinate system is established with the center of the electric campervan as the origin. The x-axis is along the horizontal direction (left-right) of the electric campervan, the y-axis is along the vertical direction (front-back) of the electric campervan, and the z-axis is perpendicular to the ground (vertical). The position of each sensor in the coordinate system is pre-calibrated. For example, the horizontal coordinates of the five sensors are -60cm, -30cm, 0cm, 30cm, and 60cm, the vertical coordinate is 120cm (i.e., the front edge of the front bumper), and the height coordinate is 25cm.

[0025] For a single target object, its spatial location is determined by combining the measurement results from multiple sensors. When multiple sensors detect the target object simultaneously, the spatial coordinates of the target object are determined using the principle of triangulation. For example, three adjacent sensors that detected the target are selected, and the coordinates of the target object are calculated using the measured distance values ​​and the known positions of the sensors. For instance, when the distance values ​​measured by the three middle sensors (located at (-30cm, 120cm, 25cm), (0cm, 120cm, 25cm), and (30cm, 120cm, 25cm)) are 150cm, 145cm, and 150cm respectively, the coordinates of the target object can be calculated to be approximately (0cm, 265cm, 25cm), indicating that the target is located 145cm directly in front of the vehicle.

[0026] The motion state of the target object is tracked in real time, and its spatial position information is updated 10 times per second. The velocity vector of the target object is calculated by comparing the spatial position difference between two adjacent samples and dividing by the sampling time interval (0.1s). For example, if the target object's position at time t1 is (0cm, 265cm, 25cm), and its position at time t2 (0.1 seconds later) is (10cm, 255cm, 25cm), then the calculated velocity in the x-direction is (10-0) / 0.1 = 100cm / s, and the velocity in the y-direction is (255-265) / 0.1 = -100cm / s. The combined velocity is [(100... 2 +100 2 )] 1 / 2 The velocity is approximately 141.4 cm / s, and the direction of motion is arctan(-1) = -45 degrees, indicating that the target object is moving to the right and rear at a speed of approximately 1.4 m / s.

[0027] Moving average filtering is used to process the raw measurement data to reduce the impact of random noise. The moving window size is set to 3, meaning that the average of the three most recent measurements is used as the current position estimate each time. Target tracking employs a nearest neighbor association method. When the deviation between the newly measured target position and the predicted position is less than a preset threshold (30cm), it is considered a continuous measurement result of the same target object; otherwise, it is judged as a new target object or a measurement anomaly.

[0028] For multi-target scenarios, cluster analysis is used to group the received echo signals according to their spatial location and identify different target objects.

[0029] In this embodiment, by uniformly arranging multiple ultrasonic sensors laterally at the front of the electric campervan to form a sensor array, it is possible to achieve omnidirectional spatial position detection of the target object, avoiding the blind spot problem of a single sensor and improving the reliability of target detection. The distance value is converted into spatial position information in a coordinate system with the electric campervan as the origin, establishing a unified coordinate reference system, which facilitates the subsequent analysis of the target's motion state and the generation of follow-up control commands. Through the coordinated operation of multiple ultrasonic sensors, the real-time coordinate information and motion state information of the target object can be obtained simultaneously, realizing a complete description of the target object's motion characteristics and providing a reliable data foundation for subsequent follow-up control.

[0030] Figure 2This diagram illustrates the comparison of target direction recognition angle deviations in embodiments of the present invention, showcasing the performance of three different algorithm schemes under different target relative direction angles (0°-330°). The proposed scheme (diamond curve) represents the YOLOv5 target detection algorithm based on deep learning, combined with Kalman filtering for target tracking. Its performance is the lowest, fluctuating between 1.2 and 2.0, peaking at approximately 2.0 at 240°. The single-sensor scheme (triangle curve) represents the traditional HOG+SVM target detection algorithm, combined with particle filtering for target tracking. It has the highest overall performance, fluctuating between 4.5 and 6.5, reaching a maximum of approximately 6.4 at 240°. The dual-sensor scheme (circular curve) represents a hybrid tracking algorithm combining CNN and optical flow methods, improving positioning accuracy through multi-sensor data fusion. Its performance is moderate, fluctuating between 3.0 and 4.3, peaking at approximately 4.3 at 240°.

[0031] All three curves exhibit periodic fluctuations, with a peak appearing approximately every 120°, indicating that the relative orientation angle of the target has a significant impact on the performance of each scheme. Around 60°, 180°, and 300°, all three schemes show relatively significant performance fluctuations. Overall, while the HOG+SVM-based single-sensor scheme achieves the highest performance value, it also exhibits the greatest fluctuation. Conversely, the YOLOv5-based scheme, although having the lowest performance value, shows a relatively stable curve with the smallest fluctuation amplitude, indicating better stability. This performance difference primarily stems from the varying robustness of different algorithms in handling changes in target attitude.

[0032] In one alternative implementation, Based on the real-time coordinate information and motion state information of the target object, the target trajectory is determined, and the target trajectory is smoothed. The filter gain is adjusted by dynamically calculating the state noise covariance matrix and the measurement noise covariance matrix. The continuous motion trajectory equation of the target object is constructed, and the target acceleration characteristics are calculated and obtained. The following control commands for the electric camping vehicle are generated based on the acceleration characteristics, including: The real-time coordinate information and motion state information of the target object are used to construct a target state vector. The target state vector is multiplied by the state transition matrix and state noise is added to obtain the state transition equation. The target trajectory is obtained using the state transition equation. The target trajectory is added to the state transition equation to determine the predicted state. The difference between the predicted state at adjacent time points is calculated to obtain the state increment. The difference between the measured value of the target trajectory and the predicted state is calculated to obtain the information sequence. The rate of change of the target trajectory within three consecutive frames is obtained. A first forgetting factor and a second forgetting factor are set based on the rate of change. The state noise covariance matrix is ​​dynamically calculated by iteratively calculating the state increment and the first forgetting factor. The measurement noise covariance matrix is ​​dynamically calculated by iteratively calculating the innovation sequence and the second forgetting factor. The filter gain is adjusted according to the state noise covariance matrix and the measurement noise covariance matrix. The filter gain is multiplied by the innovation sequence to smooth the target trajectory and obtain a smoothed target trajectory. The target trajectory after smoothing filtering is smoothed by cubic interpolation to obtain a continuous trajectory. The second difference of the continuous trajectory is calculated to obtain the target acceleration characteristics. The following control command of the electric camping vehicle is generated based on the target acceleration characteristics.

[0033] Receive real-time coordinate information and motion status information of the target object. The real-time coordinate information includes the position coordinates (x, y) of the target object, and the motion status information includes the velocity (vx, vy) of the target object.

[0034] The real-time coordinates and motion state information of the target object are used to construct a target state vector, which contains position and velocity information and can be represented as [x, y, vx, vy]. A state transition matrix, designed based on a physical motion model, is used to predict the target object's state at the next moment. The target state vector is multiplied by the state transition matrix, and state noise is added to obtain the state transition equation. The state noise represents the uncertainty in the prediction process and is considered as Gaussian white noise. The initial target trajectory is generated by recursively applying the state transition equation.

[0035] The initial target trajectory is added to the state transition equation to determine the predicted state. The predicted state represents the system's prediction of the target object's state at the next time step. The difference between the predicted states at adjacent time steps is calculated to obtain the state increment, which reflects the degree of change in the target object's state. The difference between the measured value of the target trajectory and the predicted state is calculated to obtain the innovation sequence. The innovation sequence represents the difference between the actual observed value and the predicted value.

[0036] To adapt to changes in the target trajectory, the rate of change of the target trajectory over three consecutive frames is obtained. Assuming the positions of the three consecutive frames are (x1, y1), (x2, y2), and (x3, y3), the rate of change can be obtained by calculating the rate of change of distance between adjacent positions. For example, the distance in the first segment is [((x2-x1)]... 2 +(y2-y1) 2 )] 1 / 2 The distance of the second segment is [((x3-x2)] 2 +(y3-y2) 2 )] 1 / 2 The rate of change is the ratio of the distance of the second segment to the distance of the first segment.

[0037] Based on the rate of change, a first forgetting factor and a second forgetting factor are set. When the rate of change is large, it indicates that the target object's movement has changed significantly, and a forgetting factor less than 0.9 is set, for example, 0.85. When the rate of change is small, it indicates that the target object's movement is stable, and a forgetting factor greater than 0.9 is set, for example, 0.95. For example, a threshold is set, such as setting a forgetting factor less than 0.9 if the rate of change is greater than 1.2, otherwise setting a forgetting factor greater than 0.9.

[0038] The state noise covariance matrix is ​​dynamically calculated by iteratively operating on the state increment and a first forgetting factor. For example, the outer product of the state increments and the current state noise covariance matrix are weighted and averaged according to the first forgetting factor. The measurement noise covariance matrix is ​​dynamically calculated by iteratively operating on the innovation sequence and a second forgetting factor. The outer product of the innovation sequence and the current measurement noise covariance matrix are weighted and averaged according to the second forgetting factor.

[0039] Based on the dynamically calculated state noise covariance matrix and measurement noise covariance matrix, the filter gain is adjusted. For example, prior state estimates are calculated based on the state transition equation, and the prior error covariance matrix is ​​calculated using the state noise covariance matrix. The prior error covariance matrix is ​​multiplied by the transpose of the measurement matrix, and then multiplied by the product of the measurement matrix and the prior error covariance matrix, as well as the inverse of the sum of the measurement noise covariance matrices, to obtain the filter gain at the current time. The filter gain determines the system's confidence in new observation data and is a key parameter for balancing predicted and observed values. The filter gain is then multiplied by the innovation sequence to smooth the target trajectory, resulting in the smoothed target trajectory.

[0040] For the target trajectory after smoothing filtering, cubic interpolation smoothing is performed to obtain a continuous trajectory. Cubic interpolation smoothing is a method of generating a smooth continuous curve by fitting discrete points with a cubic polynomial. Adjacent trajectory points are paired up, and a cubic polynomial function is constructed for each pair of adjacent trajectory points. The function not only passes through these two trajectory points, but also ensures that the first and second derivatives are continuous at the trajectory points, thus obtaining a series of smoothly connected cubic polynomial curve segments. In practical applications, cubic spline interpolation can be used to fit cubic polynomial curves for adjacent trajectory points. For example, for trajectory points (t1, x1, y1) and (t2, x2, y2), the system generates a cubic curve that satisfies the continuity of position and velocity.

[0041] The second-order difference of the continuous trajectory is calculated to obtain the target acceleration characteristics. The position difference between adjacent sampling points on the continuous trajectory is calculated to obtain the velocity sequence. The difference between this velocity sequence is calculated to obtain the acceleration sequence. The acceleration sequence reflects the acceleration change characteristics during the target's motion and is used to generate subsequent follow-up control commands. The second-order difference reflects the rate of change of position, i.e., acceleration. For three consecutive position points (x1, y1), (x2, y2), and (x3, y3) with a sampling time interval of Δt, the acceleration in the x-direction can be approximated as ((x3-x2)-(x2-x1)) / Δt. 2 The same applies in the y-direction.

[0042] Based on the target acceleration characteristics, follow control commands for the electric campervan are generated. The acceleration characteristics can be divided into multiple intervals, each corresponding to a different control strategy. For example, when the absolute value of the acceleration is less than 0.5 m / s²... 2 When the target is moving smoothly, a uniform speed follow command is generated; when the acceleration is between 0.5-2 m / s², the target is moving steadily. 2 When the acceleration is between 2 m / s², it indicates that the target is accelerating or decelerating, and generates corresponding acceleration or deceleration commands; when the absolute value of the acceleration is greater than 2 m / s², it indicates that the target is accelerating or decelerating. 2 When the target is rapidly accelerating or decelerating, the system generates acceleration or deceleration commands with smooth transitions to prevent the campervan from following too abruptly. Simultaneously, the direction of acceleration is considered, and the campervan's steering control is adjusted accordingly.

[0043] In this embodiment, a dynamic prediction model for target motion is established by constructing a state transition equation and introducing state noise, which improves the accuracy of target trajectory prediction. The forgetting factor is dynamically adjusted based on the rate of change of the target trajectory, which can adaptively adjust the weight of historical data, improve the response speed when the target motion is violent, and maintain the smoothness of filtering when the target motion is stable. The state noise covariance matrix and the measurement noise covariance matrix are used to dynamically adjust the filter gain. Combined with cubic interpolation smoothing, the influence of measurement noise is effectively suppressed, and a smoother target trajectory is obtained, which provides a reliable basis for generating reasonable following control commands.

[0044] In one alternative implementation, The target trajectory after smoothing filtering is smoothed by cubic interpolation to obtain a continuous trajectory. The second-order difference of the continuous trajectory is calculated to obtain the target acceleration characteristics. Based on the target acceleration characteristics, the following follow control command for the electric campervan is generated, including: The velocity vector and direction angle are calculated based on the coordinate difference between adjacent trajectory points in the target trajectory after smoothing and filtering. The velocity vector is calculated by the ratio of the Euclidean distance between adjacent trajectory points to the sampling time interval, and the direction angle is calculated by the arctangent of the coordinate increments of adjacent trajectory points. The tangential acceleration is obtained by calculating the time derivative of the velocity vector. The normal acceleration is obtained by dividing the square of the velocity vector by the radius of curvature of the target trajectory after smoothing and filtering. The 2-norm of the tangential acceleration and the normal acceleration are calculated and exponentially operated on. The compensation weight is obtained by multiplying them by the pre-set adaptive weight coefficient. The pre-set control point coefficients are dynamically adjusted using the compensation weights. The adjusted control point coefficients are then weighted and calculated with the cubic interpolation basis function. The smoothed target trajectory is then smoothed by cubic interpolation to obtain a continuous trajectory. The target acceleration features are obtained by performing a second-order difference operation on the continuous trajectory. The target acceleration features include acceleration components in the horizontal and vertical directions. The following control command for the electric campervan is generated based on the target acceleration features.

[0045] Calculate the velocity vector and direction angle from the coordinate differences between adjacent trajectory points in the smoothed target trajectory. Obtain the sequence of target trajectory points after smoothing, assuming the obtained trajectory points are P1(x1, y1), P2(x2, y2), ..., P n (x n y n For any two adjacent trajectory points P i and P i+1 Calculate the Euclidean distance dist={[(x i+1 -x i ) 2 +(y i+1 -y i ) 2 ]} 1 / 2 Considering the sampling time interval Δt, the magnitude of the velocity vector is calculated as v = dist / Δt. For example, if the coordinates of two points are P1(10, 15) and P2(13, 19), and the sampling interval is 0.1 seconds, then the Euclidean distance is 5 meters, and the magnitude of the velocity vector is 50 meters per second. Simultaneously, the direction angle θ = arctan((y...) is calculated. i+1 –y i ) / (x i+1 -x i If θ = arctan((19-15) / (13-10)) = arctan(4 / 3)≈53.13 degrees.

[0046] The tangential acceleration is obtained by calculating the time derivative of the velocity vector. The normal acceleration is obtained by dividing the square of the velocity vector by the radius of curvature of the smoothed target trajectory, and the compensation weight is calculated. For each trajectory point, the tangential acceleration 'a' is calculated based on the change in the velocity vector between preceding and following points. t =(v i+1 -v i) / Δt. For example, if the velocities at two consecutive moments are 50 m / s and 55 m / s respectively, and the time interval is 0.1 seconds, then the tangential acceleration is 50 m / s². 2 The radius of curvature R of the trajectory is calculated using a three-point circle fitting method. A circle can be defined using the coordinates of three consecutive trajectory points P1, P2, and P3; the radius of this circle is the radius of curvature of the trajectory. Normal acceleration a n =v 2 / R. For example, if the velocity is 50 m / s and the radius of curvature is 100 m, then the normal acceleration is 25 m / s². 2 The tangential and normal accelerations are calculated using the L2 norm: a = (a t 2 +a n 2 ), in the example above it is [(50 2 +25 2 )] 1 / 2 =55.9m / s 2 The compensation weight w = α·exp(β·a), where α is a pre-set adaptive weight coefficient, for example, α = 0.8, and β is an adjustment parameter, for example, β = 0.01. Substituting into the example above, the compensation weight w = 0.8·exp(0.01·55.9) ≈ 1.26.

[0047] The pre-set control point coefficients are dynamically adjusted using compensation weights. The adjusted control point coefficients are then weighted and calculated with a cubic interpolation basis function. This weighted average is used to smooth the filtered target trajectory using cubic interpolation to obtain a continuous trajectory. The pre-set control point coefficients are c0, c1, c2, and c3, typically with initial values ​​of c0=1, c1=0.5, c2=0.3, and c3=0.2. The coefficients are dynamically adjusted according to the compensation weights: c' i =c i For example, if w = 1.26, then the adjusted control point coefficients are c'0 = 1 * 1.26 = 1.26, c'1 = 0.5 * 1.26 = 0.63, c'2 = 0.3 * 1.26 = 0.378, and c'3 = 0.2 * 1.26 = 0.252. The cubic interpolation basis functions include B0(t) = (1-t). 3 B1(t) = 3t(1-t) 2 B2(t) = 3t 2 (1-t), B3(t)=t 3, where t is a parameter ranging from [0, 1]. For each trajectory segment, using control points P0, P1, P2, P3 and adjusted control point coefficients c'0, c'1, c'2, c'3, the interpolation point P(t) = c'0·P0·B0(t) + c'1·P1·B1(t) + c'2·P2·B2(t) + c'3·P3·B3(t) can be calculated. By taking a sufficient number of t values ​​on each trajectory segment (e.g., increasing by 0.05 each time), dense interpolation points can be obtained, forming a continuous and smooth trajectory.

[0048] A second-order difference operation is performed on the continuous trajectory to obtain the target acceleration characteristics, which include horizontal and vertical acceleration components. Following control commands for the electric campervan are generated based on these target acceleration characteristics. For the interpolated continuous trajectory point sequence Q1, Q2, ..., Q... m Calculate the second-order difference: a_x = (x i+2 -2x i+1 +x i ) / Δt 2 For horizontal acceleration, a_y = (y i+2 -2y i+1 +y i ) / Δt 2 This represents the acceleration in the vertical direction. For example, if the x-coordinates of three consecutive points are 10, 15, and 18, and the time interval is 0.1 seconds, then the horizontal acceleration is (18 - 2·15 + 10) / 0.1. 2 =-20 / 0.01=-2000m / s 2 The vertical acceleration is calculated. Based on the calculated target acceleration characteristics, follow control commands for the electric campervan are generated. For example, when target deceleration (negative acceleration) is detected, a deceleration command is generated; when target turning (change in horizontal acceleration) is detected, a turning command is generated; and when target acceleration (positive acceleration) is detected, an acceleration command is generated. For instance, if the detected horizontal acceleration is -2000 m / s²... 2 The vertical acceleration is 500 m / s². 2 It can generate a "decelerate and turn right" control command and send it to the actuator of the electric campervan.

[0049] In existing technologies, interpolation smoothing of target trajectories usually uses fixed control point coefficients, which cannot be adaptively adjusted according to changes in the target's motion state. This can easily lead to over-smoothing or tracking lag in the interpolation results when the target's motion changes drastically. This embodiment obtains the velocity vector and direction angle by calculating the coordinate difference between adjacent trajectory points, and then solves for the tangential acceleration and normal acceleration. Based on these motion characteristics, compensation weights are constructed to realize the dynamic adjustment of the control point coefficient. The adaptive adjustment mechanism enables the interpolation process to automatically adjust the smoothness according to the intensity of the target motion. When the target motion is stable, a larger control point coefficient ensures the smoothness of the trajectory, while when the target motion is intense, a smaller control point coefficient improves the tracking accuracy of the trajectory. This embodiment not only solves the problem of excessive smoothing or tracking lag caused by fixed control point coefficients, but also improves the tracking response speed while ensuring trajectory smoothness, so that the generated follow control commands can better adapt to the dynamic changes in the target's motion state, thereby improving the follow control performance of the electric camping vehicle.

[0050] In one alternative implementation, The system acquires environmental information during the electric campervan's journey and determines whether obstacles exist. If obstacles exist, the current environment is divided into grid cells, and a passage cost is assigned to each grid cell. A path planning tree is constructed, a comprehensive score for each path is determined, and the optimal obstacle avoidance path is selected based on the comprehensive score. The following control commands are then modified, including: The system acquires environmental information during the operation of the electric campervan, calculates the number of data points within a preset distance radius around each data point in the environmental information to obtain the data point density, marks data points with a density greater than a preset density threshold as core points, calculates the spatial distance between each core point, classifies core points with a spatial distance less than a preset distance radius into the same category, compares the number of data points in each category with a preset number threshold to obtain the obstacle judgment result, and extracts the center position and size from the obstacle to form an obstacle feature vector based on the obstacle judgment result. The current environment is divided into grid cells. The distance from each grid cell to the nearest obstacle is calculated based on the obstacle feature vector to obtain the obstacle occupancy cost. The distance from each grid cell to the target point is calculated to obtain the distance cost. The obstacle occupancy cost and the distance cost are weighted and combined to obtain the passage cost of each grid cell. An extended tree is generated from the start and end points. Sampling points are randomly sampled in the current environment. The nearest node is found in the extended tree. The sampling points are used to build extended branches according to a preset step size to obtain multiple connectable paths. The cumulative value of the travel cost of each connectable path is calculated based on the travel cost of each grid cell. A comprehensive score is calculated by weighting the length of each connectable path, the cumulative value of the passage cost, and the curvature. The optimal obstacle avoidance path is selected based on the comprehensive score, and the following control command is modified according to the optimal obstacle avoidance path.

[0051] Environmental information is acquired and obstacle detection is performed. Environmental information is acquired through sensors mounted on the electric campervan, including LiDAR and cameras. The output data forms a 3D point cloud or a 2D image to represent the current environment. After acquiring data from the sensors, each data point is processed. For each data point, the number of data points within a 10cm radius is calculated to obtain the density value of that point. For example, if there are 15 other data points within a 10cm radius of a data point, the density value of that point is 15. Data points with a density value greater than 8 are marked as core points. The spatial distance between all core points is calculated, and core points with a distance less than 15cm are grouped into the same category. If an area has 10 core points, and the distance between them is less than 15cm, these 10 core points are grouped into the same category. For each formed category, the total number of data points it contains is counted. If the total number of data points exceeds 30, the category is determined to be an obstacle. For each identified obstacle, its center position and size are calculated. The center position is obtained by calculating the average coordinates of all data points within the obstacle, and the size is determined by calculating the maximum range of the obstacle in each dimension. For example, the center of an obstacle is (5 meters, 3 meters), and its dimensions are 0.8 meters long and 0.6 meters wide.

[0052] After obstacles are identified, the current environment is divided into grid cells. The environment is divided into square grids of 20 cm x 20 cm. For each grid cell, the passage cost is calculated based on the feature vectors of the identified obstacles, and the distance from the grid cell to the nearest obstacle is calculated to obtain the obstacle occupancy cost. For example, if the distance from a grid cell to the nearest obstacle is 0.5 meters, its obstacle occupancy cost can be set to 20. The closer the distance, the higher the occupancy cost. When the distance is less than the safety threshold of 0.3 meters, the occupancy cost is set to 100, indicating that the area is impassable. The distance from the grid cell to the target point is then calculated to obtain the distance cost. For example, if the distance from a grid cell to the target point is 10 meters, its distance cost can be set to 50. The farther the distance, the higher the distance cost. The obstacle occupancy cost and the distance cost are weighted and combined in a ratio of 0.7:0.3 to obtain the comprehensive passage cost of the grid cell. For example, if the obstacle occupancy cost of a certain grid cell is 20 and the distance cost is 50, then its total passage cost is 20×0.7+50×0.3=29.

[0053] The path planning uses a fast expanding random tree algorithm. Starting from the starting point, an expanding tree is built, randomly generating sampling points in the current environment. For example, a sampling point is generated at coordinates (8 meters, 6 meters). The nearest node to this sampling point is found in the current expanding tree. Assuming the nearest node is located at (7.5 meters, 5.8 meters), the tree expands towards the sampling point with a step size of 0.5 meters, generating a new node at (7.85 meters, 6.05 meters). It is checked whether the path from the nearest node to the new node passes through a high-cost region. If the passage cost of all grid cells on the path is less than 70, the path is considered feasible, and the new node is added to the expanding tree. This process is repeated, also building an expanding tree from the destination. When the two trees are connected, a complete path is formed.

[0054] For each connectable path, a comprehensive score is calculated. The comprehensive score considers three factors: path length, cumulative travel cost, and path curvature. Path length is the sum of the lengths of all line segments on the path; for example, a path might be 15 meters long. The cumulative travel cost is the sum of the travel costs of all grid cells the path passes through; for example, a path might have a cumulative travel cost of 320. Path curvature is calculated by determining the angle change between adjacent line segments on the path; for example, a path might have an average curvature of 0.15 radians / meter. These three factors are weighted and combined according to a weighting of 0.3:0.5:0.2 to obtain the comprehensive score of the path. For example, if a path is 15 meters long, has a cumulative travel cost of 320, and an average curvature of 0.15 radians / meter, its comprehensive score is 15 × 0.3 + 320 × 0.5 + 0.15 × 0.2 = 164.53. The path with the lowest comprehensive score is selected as the optimal obstacle avoidance path.

[0055] The following control command is modified based on the selected optimal obstacle avoidance path. The optimal path is converted into a series of control points, each containing position and direction information. For example, control points on the path might include (5 meters, 3 meters, 0 degrees), (6 meters, 3.5 meters, 15 degrees), etc. Based on the positional and directional deviations between the electric campervan's current position and the nearest control point, the required speed and steering adjustments are calculated, thus correcting the original following control command. For instance, if the current positional deviation is 0.2 meters and the directional deviation is 5 degrees, it might require a 10% reduction in speed and a 3-degree right turn. The corrected control command is then sent to the electric campervan's drive and steering systems to achieve obstacle avoidance.

[0056] In this embodiment, obstacles in the environment are identified by a clustering method based on data point density, and the center position and size features of the obstacles are extracted, achieving accurate detection and feature extraction of obstacles in the environment. This provides reliable environmental information for subsequent obstacle avoidance path planning. The current environment is divided using a gridding method, and a reasonable passage cost evaluation system is established by comprehensively considering obstacle occupancy cost and distance cost. This allows path planning to optimize path length while ensuring a safe distance. By using a bidirectional extended tree path search method, combined with a comprehensive scoring mechanism for path length, passage cost, and curvature, a smooth and safe obstacle avoidance path can be quickly generated. Based on this, the following control command is reasonably modified, ensuring the safe following performance of the electric camping vehicle in complex environments.

[0057] Figure 3 This is a logic block diagram of obstacle avoidance path planning for an electric camping vehicle according to an embodiment of the present invention.

[0058] In one alternative implementation, Generate an extended tree from the start and end points, randomly sample points in the current environment, find the nearest node in the extended tree, and construct extended branches from the sample points according to a preset step size to obtain multiple connectable paths, including: In an environment with obstacles, an extended tree is generated from the starting point and the ending point respectively. A line of sight is established from the starting point to the ending point. The set of intersection points between the line of sight and the obstacles is calculated. At each intersection point, the channel geometric features are extracted. The channel geometric features include the channel width indicating the minimum distance between obstacles, the channel direction vector indicating the tangent vector of the channel centerline, and the channel center point indicating the center position of the narrowest point of the channel. Calculate the distance from the location in the environment to the nearest channel center point, divide the distance by the channel width and take the negative exponent to obtain the attenuation factor, multiply the attenuation factor by the difference between the channel direction vector and the location projection vector and normalize it to obtain the guiding vector of the location in the environment. Sampling points are randomly sampled in the current environment. The normalized distance from the sampling point to the nearest channel center is multiplied by the first weight coefficient. The cosine similarity between the sampling direction and the guiding vector is multiplied by the second weight coefficient. The two are added together to obtain the sampling point score. Valid sampling points that meet the preset score threshold are selected based on the sampling point score. The first weight coefficient and the second weight coefficient are preset and their sum is 1. Find the nearest node to the effective sampling point in each of the two extended trees, and build extended branches from the effective sampling point to the two nearest nodes according to a preset step size to obtain new nodes. Add the new nodes to the corresponding extended trees, and repeat the addition until the extended trees are built to obtain multiple connectable paths.

[0059] In an environment with obstacles, two expansion trees are generated from the starting point and the ending point respectively. These two trees are progressively expanded and attempted to connect through subsequent processes, establishing a straight line connection from the starting point to the ending point, called the line-of-sight connection. By calculating the set of intersection points between the connection line and obstacles in the environment, the obstacle area that needs to be bypassed is determined. At each intersection point, the channel geometry features are extracted. The channel geometry features include three key components: channel width, channel direction vector, and channel center point. The channel width indicates the minimum distance between obstacles; for example, at a specific intersection point, if the minimum distance between two obstacles is 3 meters, then the channel width at that point is 3 meters. The channel direction vector indicates the tangent vector of the channel centerline, indicating the direction of the channel's extension. The channel center point identifies the coordinates of the center position at the narrowest point of the channel.

[0060] Calculate the distance from any location in the environment to the nearest channel center point. For example, if a point's coordinates are (5, 4) and the nearest channel center point's coordinates are (6, 5), the Euclidean distance is calculated to be 1.414. Divide this distance by the corresponding channel width; for example, if the channel width is 3 meters, the ratio is 0.471. Take the negative exponent of this ratio to obtain the attenuation factor, which is e^(-0.471), approximately equal to 0.624. Then calculate the position projection vector, i.e., the unit vector pointing from the position to the channel center point. Multiply the attenuation factor by the difference between the channel direction vector and the position projection vector, and normalize the result to obtain the guide vector for that location. The guide vector is used to guide the tree's growth direction, making it more inclined to expand along the channel direction.

[0061] Random sampling is performed in the environmental space to obtain sampling points. For each sampling point, the normalized distance from the sampling point to the nearest channel center is calculated. Normalization is achieved by dividing the actual distance by the maximum size of the environment. For example, if the environment is 100×100 meters and the sampling point is 10 meters from the channel center, the normalized distance is 0.1. The normalized distance is multiplied by a preset first weighting coefficient, for example, 0.4. Simultaneously, the cosine similarity between the sampling direction and the guiding vector is calculated, with a value ranging from [-1, 1]. The larger the value, the more consistent the directions. The similarity is multiplied by a second weighting coefficient of 0.6 and added to the weighted distance to obtain the comprehensive score of the sampling point. A preset score threshold of 0.5 is set, and sampling points with a score greater than or equal to 0.5 are selected as valid sampling points.

[0062] Find the nearest node to the valid sampling point in each of the two expanded trees using a KD-tree data structure. Assume the nearest node found from the starting tree has coordinates (15, 20), the nearest node found from the ending tree has coordinates (25, 18), and the valid sampling point has coordinates (20, 22). Extend branches from the valid sampling point to the two nearest nodes by a preset step size (e.g., 1 meter). For example, calculate the unit vector from the nearest node to the sampling point, and move the nearest node's coordinates along this vector by the preset step size to obtain a new node. For instance, moving the nearest node from the starting tree 1 meter towards the sampling point yields a new node with coordinates (15.77, 20.85); the ending tree yields a new node with coordinates (24.42, 18.89). Add the new nodes to their respective expanded trees and establish parent-child relationships for subsequent path backtracking.

[0063] Repeat the sampling, filtering, and expansion process described above until there are directly connectable node pairs in the two trees (i.e., no obstacles between the two nodes and a distance less than a preset connection threshold, such as 2 meters), or until the maximum number of iterations is reached (such as 5000). Each successful connection of node pairs between the two trees forms a feasible path. By backtracking the parent-child relationships in the two trees, a complete path from the starting point to the ending point is constructed, and multiple connectable paths are obtained for subsequent optimization.

[0064] In this embodiment, by extracting channel geometric features at obstacle intersections, including channel width, direction vector, and center point, a complete description of the environmental passage characteristics is constructed, providing a more geometrically meaningful environmental representation for subsequent path planning. Based on the channel geometric features, a guidance vector field is constructed, and the guidance intensity is adaptively adjusted through distance attenuation factor and direction projection, enabling the path planning process to naturally avoid narrow areas of obstacles and extend along the channel direction. A score-based sampling point selection mechanism is adopted, comprehensively considering the distance from the sampling point to the channel center and the consistency between the sampling direction and the guidance vector, improving the quality of effective sampling points, making the growth of the expansion tree more targeted, and enabling the rapid construction of a set of connectable paths that conform to environmental characteristics, thereby improving the efficiency and quality of path planning.

[0065] In one alternative implementation, The revised follow control commands control the electric campervan's power and steering systems to follow a target, including: Receive a modified follow control command, wherein the modified follow control command includes a speed value and a steering angle value; The electric campervan's power system is controlled to adjust its speed according to the speed value in the revised follow control command; The steering system of the electric campervan is controlled to adjust its steering according to the steering angle value in the modified follow control command, so as to enable the electric campervan to follow the target.

[0066] The system receives corrected follow control commands, which are transmitted wirelessly from the remote control or the transmitter on the target object. The communication module uses Bluetooth 5.0 or 4G / 5G mobile communication technology to receive these commands. The corrected follow control commands contain two key parameters: speed and steering angle. The speed value, in km / h, represents the desired speed of the electric campervan; the steering angle value, in degrees, represents the required steering angle. The received commands are stored in the control system's cache, awaiting reading and processing by the central processing unit.

[0067] In terms of speed control, the central processing unit (CPU) reads the speed value from the corrected follow control command in the cache. This speed value is used to calculate the power output required by the powertrain. For example, assuming the received speed value is 5 km / h and the electric campervan's current speed is 3 km / h, the control system calculates a speed increase of 2 km / h. Considering the electric campervan's weight of 2500 kg, to achieve smooth acceleration, the system will linearly increase the motor output power over 2 seconds, gradually increasing from the current power level to the target power level. After receiving the power control signal from the CPU, the power control module adjusts the duty cycle of the motor controller's PWM signal to change the motor output power. For example, increasing the PWM signal duty cycle from 40% to 60% increases the motor output power. Simultaneously, Hall effect sensors installed on the wheels monitor the vehicle speed in real time. When the error between the actual speed and the target speed is less than 0.2 km / h, the speed adjustment is considered to have met the requirements.

[0068] In terms of steering control, the central processing unit reads the steering angle value from the corrected follow control command. Assuming the received steering angle is 15 degrees (right turn), and the electric campervan's current steering angle is 0 degrees (straight), the steering control module controls the steering system to perform the steering operation based on this information. The electric campervan uses an electric power steering system, achieving steering by controlling the rotation angle and torque of the steering motor. The steering control module converts the steering angle value into the number of rotation steps of the steering motor; for example, a 15-degree steering angle corresponds to 450 motor rotation steps (assuming a motor step angle of 0.9 degrees and a reduction ratio of 27:1). To ensure smooth steering, a steering rate control algorithm is designed, controlling the steering rate to not exceed 10 degrees / second; therefore, completing a 15-degree turn takes approximately 1.5 seconds. During steering, the angle sensor located on the steering column provides real-time feedback on the actual steering angle. When the error between the actual steering angle and the target steering angle is less than 0.5 degrees, the steering adjustment is considered to have met the requirements.

[0069] To achieve precise tracking of the target by the electric campervan, a closed-loop feedback control mechanism was designed. In the speed control stage, the actual speed of the electric campervan is measured in real time using wheel speed sensors and compared with the target speed value to calculate the speed error. When the speed error exceeds ±0.5 km / h, the motor output power is automatically adjusted to bring the actual speed closer to the target speed. For example, when the detected actual speed is 5.7 km / h and the target speed is 5.0 km / h, the PWM signal duty cycle is reduced from 60% to 55%, decreasing the motor output power. Similarly, in the steering control stage, the actual steering angle is measured in real time using a steering angle sensor and compared with the target steering angle to calculate the steering angle error. When the steering angle error exceeds ±1 degree, the output of the steering motor is automatically adjusted to bring the actual steering angle closer to the target steering angle.

[0070] In this embodiment, by decomposing the modified follow control command into two control quantities—speed value and steering angle value—the motion control of the electric campervan is decoupled, simplifying the control logic and improving the execution efficiency of the control command. Based on the speed value, the power system is directly controlled, and combined with the steering angle value, the steering system is precisely adjusted, achieving coordinated control of the electric campervan's motion state and ensuring the smoothness of motion during the following process. Through the cooperation of the power system and the steering system, the electric campervan can adjust its motion state in a timely manner according to the modified control command, accurately execute obstacle avoidance and following tasks, and improve the following control accuracy and reliability of the electric campervan in complex environments.

[0071] In one alternative embodiment, A safety mechanism is in place to automatically limit the speed to a safe range when the received speed value exceeds a safety threshold (e.g., 10 km / h). Similarly, when the received steering angle value exceeds the maximum steering angle (e.g., 30 degrees), the steering angle is limited to a safe range. Furthermore, an obstacle detection function is included, using ultrasonic sensors and cameras installed around the vehicle to monitor the surrounding environment in real time. When an obstacle is detected within 3 meters, the vehicle automatically reduces its speed; when the distance is less than 1 meter, it automatically stops to prevent collisions.

[0072] To adapt to different road conditions and load situations, an adaptive control strategy is also implemented. For example, on uphill sections, the motor output power is automatically increased based on the gradient value detected by the gradient sensor to ensure that the target speed is maintained. Under different load conditions (such as different passenger loads), the power output and steering sensitivity are automatically adjusted based on the detected total vehicle weight to ensure the accuracy and stability of the following control.

[0073] A second aspect of the present invention provides an ultrasonic-based automatic following control system for electric campervans, comprising: The first unit is used to collect the spatial location information of the target object through the ultrasonic sensor array of the electric camping vehicle, and to determine the real-time coordinate information and motion status information of the target object; The second unit is used to determine the target trajectory based on the real-time coordinate information and motion state information of the target object, smooth the target trajectory, adjust the filter gain by dynamically calculating the state noise covariance matrix and the measurement noise covariance matrix, construct the continuous motion trajectory equation of the target object and calculate and obtain the target acceleration characteristics, and generate the following control command of the electric camping vehicle based on the acceleration characteristics. The third unit is used to send the follow control command to the power system and steering system of the electric campervan; The fourth unit is used to acquire environmental information during the electric camper's journey and determine whether there are obstacles. If there are obstacles, the current environment is divided into grid cells and a passage cost is assigned to each grid cell. A path planning tree is constructed, the comprehensive score of each path is determined, and the optimal obstacle avoidance path is selected based on the comprehensive score. The following control command is then modified. The fifth unit is used to control the power system and steering system of the electric campervan to follow the target according to the revised follow control instructions.

[0074] A third aspect of the present invention provides an electronic device, comprising: A processor and a memory for storing processor-executable instructions, wherein the processor is configured to invoke instructions stored in the memory to perform the aforementioned method.

[0075] A fourth aspect of the present invention provides a computer-readable storage medium having stored thereon computer program instructions that, when executed by a processor, implement the aforementioned method.

[0076] This invention can be a method, apparatus, system, and / or computer program product. The computer program product may include a computer-readable storage medium having computer-readable program instructions loaded thereon for performing various aspects of the invention.

[0077] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, and not to limit them; although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some or all of the technical features; and these modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the scope of the technical solutions of the embodiments of the present invention.

Claims

1. An ultrasonic-based automatic following control method for electric camping vehicles, characterized in that, include: The spatial location information of the target object is collected by the ultrasonic sensor array of the electric camping vehicle to determine the real-time coordinate information and motion status information of the target object; The target trajectory is determined based on the real-time coordinate information and motion state information of the target object. The target trajectory is smoothed. The filter gain is adjusted by dynamically calculating the state noise covariance matrix and the measurement noise covariance matrix. The continuous motion trajectory equation of the target object is constructed and the target acceleration characteristics are calculated and obtained. The following control command of the electric camping vehicle is generated based on the acceleration characteristics. The follow control command is sent to the power system and steering system of the electric campervan; The system acquires environmental information during the electric camper's journey and determines whether there are obstacles. If there are obstacles, the current environment is divided into grid cells, and a passage cost is assigned to each grid cell. A path planning tree is constructed, a comprehensive score for each path is determined, and the optimal obstacle avoidance path is selected based on the comprehensive score. The following control command is then modified. The electric campervan's power and steering systems are controlled according to the revised follow control commands to follow the target.

2. The method according to claim 1, characterized in that, The spatial location information of the target object is collected by the ultrasonic sensor array of the electric camping vehicle, and the real-time coordinate information and motion status information of the target object are determined, including: The ultrasonic sensor array at the front of the electric campervan emits ultrasonic signals and receives reflected echo signals. The ultrasonic sensor array includes multiple ultrasonic sensors that are evenly arranged laterally along the front of the electric campervan. The distance between the target object and each ultrasonic sensor is calculated based on the received reflected echo signal, and the distance value is converted into spatial position information in a coordinate system with the electric camper vehicle as the origin. Based on the spatial location information, the real-time coordinates and motion status information of the target object relative to the electric camper are determined. The motion status information includes the target object's speed and direction of motion.

3. The method according to claim 1, characterized in that, Based on the real-time coordinate information and motion state information of the target object, the target trajectory is determined, and the target trajectory is smoothed. The filter gain is adjusted by dynamically calculating the state noise covariance matrix and the measurement noise covariance matrix. The continuous motion trajectory equation of the target object is constructed, and the target acceleration characteristics are calculated and obtained. The following control commands for the electric camping vehicle are generated based on the acceleration characteristics, including: The real-time coordinate information and motion state information of the target object are used to construct a target state vector. The target state vector is multiplied by the state transition matrix and state noise is added to obtain the state transition equation. The target trajectory is obtained using the state transition equation. The target trajectory is added to the state transition equation to determine the predicted state. The difference between the predicted state at adjacent time points is calculated to obtain the state increment. The difference between the measured value of the target trajectory and the predicted state is calculated to obtain the information sequence. The rate of change of the target trajectory within three consecutive frames is obtained. A first forgetting factor and a second forgetting factor are set based on the rate of change. The state noise covariance matrix is ​​dynamically calculated by iteratively calculating the state increment and the first forgetting factor. The measurement noise covariance matrix is ​​dynamically calculated by iteratively calculating the innovation sequence and the second forgetting factor. The filter gain is adjusted according to the state noise covariance matrix and the measurement noise covariance matrix. The filter gain is multiplied by the innovation sequence to smooth the target trajectory and obtain a smoothed target trajectory. The target trajectory after smoothing filtering is smoothed by cubic interpolation to obtain a continuous trajectory. The second difference of the continuous trajectory is calculated to obtain the target acceleration characteristics. The following control command of the electric camping vehicle is generated based on the target acceleration characteristics.

4. The method according to claim 3, characterized in that, The target trajectory after smoothing filtering is smoothed by cubic interpolation to obtain a continuous trajectory. The second-order difference of the continuous trajectory is calculated to obtain the target acceleration characteristics. Based on the target acceleration characteristics, the following follow control command for the electric campervan is generated, including: The velocity vector and direction angle are calculated based on the coordinate difference between adjacent trajectory points in the target trajectory after smoothing and filtering. The velocity vector is calculated by the ratio of the Euclidean distance between adjacent trajectory points to the sampling time interval, and the direction angle is calculated by the arctangent of the coordinate increments of adjacent trajectory points. The tangential acceleration is obtained by calculating the time derivative of the velocity vector. The normal acceleration is obtained by dividing the square of the velocity vector by the radius of curvature of the target trajectory after smoothing and filtering. The 2-norm of the tangential acceleration and the normal acceleration are calculated and exponentially operated on. The compensation weight is obtained by multiplying them by the pre-set adaptive weight coefficient. The pre-set control point coefficients are dynamically adjusted using the compensation weights. The adjusted control point coefficients are then weighted and calculated with the cubic interpolation basis function. The smoothed target trajectory is then smoothed by cubic interpolation to obtain a continuous trajectory. The target acceleration features are obtained by performing a second-order difference operation on the continuous trajectory. The target acceleration features include acceleration components in the horizontal and vertical directions. The following control command for the electric campervan is generated based on the target acceleration features.

5. The method according to claim 1, characterized in that, The system acquires environmental information during the electric campervan's journey and determines whether obstacles exist. If obstacles exist, the current environment is divided into grid cells, and a passage cost is assigned to each grid cell. A path planning tree is constructed, a comprehensive score for each path is determined, and the optimal obstacle avoidance path is selected based on the comprehensive score. The following control commands are then modified, including: The system acquires environmental information during the operation of the electric campervan, calculates the number of data points within a preset distance radius around each data point in the environmental information to obtain the data point density, marks data points with a density greater than a preset density threshold as core points, calculates the spatial distance between each core point, classifies core points with a spatial distance less than a preset distance radius into the same category, compares the number of data points in each category with a preset number threshold to obtain the obstacle judgment result, and extracts the center position and size from the obstacle to form an obstacle feature vector based on the obstacle judgment result. The current environment is divided into grid cells. The distance from each grid cell to the nearest obstacle is calculated based on the obstacle feature vector to obtain the obstacle occupancy cost. The distance from each grid cell to the target point is calculated to obtain the distance cost. The obstacle occupancy cost and the distance cost are weighted and combined to obtain the passage cost of each grid cell. An extended tree is generated from the start and end points. Sampling points are randomly sampled in the current environment. The nearest node is found in the extended tree. The sampling points are used to build extended branches according to a preset step size to obtain multiple connectable paths. The cumulative value of the travel cost of each connectable path is calculated based on the travel cost of each grid cell. A comprehensive score is calculated by weighting the length of each connectable path, the cumulative value of the passage cost, and the curvature. The optimal obstacle avoidance path is selected based on the comprehensive score, and the following control command is modified according to the optimal obstacle avoidance path.

6. The method according to claim 5, characterized in that, Generate an extended tree from the start and end points, randomly sample points in the current environment, find the nearest node in the extended tree, and construct extended branches from the sample points according to a preset step size to obtain multiple connectable paths, including: In an environment with obstacles, an extended tree is generated from the starting point and the ending point respectively. A line of sight is established from the starting point to the ending point. The set of intersection points between the line of sight and the obstacles is calculated. At each intersection point, the channel geometric features are extracted. The channel geometric features include the channel width indicating the minimum distance between obstacles, the channel direction vector indicating the tangent vector of the channel centerline, and the channel center point indicating the center position of the narrowest point of the channel. Calculate the distance from the location in the environment to the nearest channel center point, divide the distance by the channel width and take the negative exponent to obtain the attenuation factor, multiply the attenuation factor by the difference between the channel direction vector and the location projection vector and normalize it to obtain the guiding vector of the location in the environment. Sampling points are randomly sampled in the current environment. The normalized distance from the sampling point to the nearest channel center is multiplied by the first weight coefficient. The cosine similarity between the sampling direction and the guiding vector is multiplied by the second weight coefficient. The two are added together to obtain the sampling point score. Valid sampling points that meet the preset score threshold are selected based on the sampling point score. The first weight coefficient and the second weight coefficient are preset and their sum is 1. Find the nearest node to the effective sampling point in each of the two extended trees, and build extended branches from the effective sampling point to the two nearest nodes according to a preset step size to obtain new nodes. Add the new nodes to the corresponding extended trees, and repeat the addition until the extended trees are built to obtain multiple connectable paths.

7. The method according to claim 1, characterized in that, The revised follow control commands control the electric campervan's power and steering systems to follow a target, including: Receive a modified follow control command, wherein the modified follow control command includes a speed value and a steering angle value; The electric campervan's power system is controlled to adjust its speed according to the speed value in the revised follow control command; The steering system of the electric campervan is controlled to adjust its steering according to the steering angle value in the modified follow control command, so as to enable the electric campervan to follow the target.

8. An ultrasonic-based automatic following control system for electric camping vehicles, used to implement the method of any one of claims 1-7, characterized in that, include: The first unit is used to collect the spatial location information of the target object through the ultrasonic sensor array of the electric camping vehicle, and to determine the real-time coordinate information and motion status information of the target object; The second unit is used to determine the target trajectory based on the real-time coordinate information and motion state information of the target object, smooth the target trajectory, adjust the filter gain by dynamically calculating the state noise covariance matrix and the measurement noise covariance matrix, construct the continuous motion trajectory equation of the target object and calculate and obtain the target acceleration characteristics, and generate the following control command of the electric camping vehicle based on the acceleration characteristics. The third unit is used to send the follow control command to the power system and steering system of the electric campervan; The fourth unit is used to acquire environmental information during the electric camper's journey and determine whether there are obstacles. If there are obstacles, the current environment is divided into grid cells and a passage cost is assigned to each grid cell. A path planning tree is constructed, the comprehensive score of each path is determined, and the optimal obstacle avoidance path is selected based on the comprehensive score. The following control command is then modified. The fifth unit is used to control the power system and steering system of the electric campervan to follow the target according to the revised follow control instructions.

9. An electronic device, characterized in that, include: processor; Memory used to store processor-executable instructions; The processor is configured to invoke instructions stored in the memory to execute the method according to any one of claims 1 to 7.

10. A computer-readable storage medium having computer program instructions stored thereon, characterized in that, When the computer program instructions are executed by the processor, they implement the method described in any one of claims 1 to 7.

Images