Autonomous navigation multi-sensor robot time-frequency domain trajectory planning signal processing method

Abstract

According to the time-frequency domain trajectory planning signal processing method for the autonomous navigation multi-sensor robot, the robot carries various sensors such as a high-definition camera, an ultrasonic sensor and an infrared sensor, the surrounding environment is monitored in real time, and time-frequency domain signal data are collected. Preprocessing and feature extraction are performed on the collected signals, an interference source is identified by using a decision tree, interference signals are filtered by using a spatial filtering technology, and the signal quality is improved. And constructing a trajectory planning model based on the signal after interference filtering, optimizing the trajectory planning signal through curve fitting collaborative operation, and evaluating the signal quality and stability at the same time. And according to the optimized trajectory planning signal, a robot motion trajectory is generated in real time, trajectory planning parameters are dynamically adjusted according to environment changes and robot states, and it is ensured that the robot stably and accurately executes tasks.

Description

Technical Field

[0001] The present invention relates to the field of robots, and particularly to a time-frequency domain trajectory planning signal processing method for autonomous navigation multi-sensor robots. Background Art

[0002] In today's technological field, the application of robots is becoming increasingly widespread, especially in scenarios that require interaction with complex environments and the execution of precise tasks. As an important branch among them, the development of multi-sensor robots has attracted much attention. Traditional robot environmental perception and signal processing methods often have many limitations. In terms of sensor configuration, a single type of sensor is difficult to comprehensively and accurately obtain environmental information. For example, relying solely on visual sensors may be affected by factors such as lighting and occlusion, resulting in incomplete or inaccurate information acquisition; while using only distance sensors cannot provide a rich description of environmental features.

[0003] In terms of signal acquisition, previous methods may have problems such as insufficient sampling frequency and insufficient data accuracy, making it difficult to meet the requirements for capturing rapid environmental changes and fine features. Moreover, the raw signals collected usually contain a large amount of noise and interference, seriously affecting subsequent analysis and processing. For signal processing and feature extraction, the efficiency and accuracy of traditional methods need to be improved. Common filtering and denoising methods may lose some useful signals while removing noise, resulting in information loss. Feature extraction algorithms may not be able to fully mine the key features in the signals, affecting subsequent decision-making and control. In terms of trajectory planning, early models were often too simple to adapt to complex and changing environments. Moreover, their resistance to interference and noise is weak, easily leading to deviations and errors in trajectory planning.

[0004] Existing multi-sensor robot technologies have many deficiencies in sensor configuration, signal acquisition, processing, and trajectory planning, and cannot meet the increasing application requirements. There is an urgent need for a more advanced, efficient, and accurate multi-sensor robot technology to achieve comprehensive environmental perception, accurate signal processing, and optimized trajectory planning, thereby improving the performance and application scope of robots. Summary of the Invention

[0005] To solve the above problems, the present invention proposes a time-frequency domain trajectory planning signal processing method for autonomous navigation multi-sensor robots, and the specific steps are as follows:

[0006] Step 1: Environmental perception and signal acquisition of the multi-sensor robot; the robot is equipped with a variety of sensors such as a high-definition camera, an ultrasonic sensor, and an infrared sensor to work together, continuously monitor the surrounding environment, and collect relevant time-frequency domain signal data;

[0007] Step 2: Signal preprocessing and feature extraction; Preprocess the collected signals, including operations such as denoising and filtering, and then use methods such as wavelet analysis and Fourier transform to extract the time-frequency features of the signals, providing data support for subsequent trajectory planning;

[0008] Step 3: Interference source identification and noise filtering; Use decision trees to identify the signal features extracted, distinguish useful signals from interference signals; then adopt spatial filtering technology to filter out the interference signals and improve the signal quality;

[0009] Step 4: Trajectory planning model construction; Based on the signals after filtering out interference, construct a trajectory planning model with curve fitting for collaborative operation, and use a high-precision curve fitting algorithm to optimize the trajectory planning signals;

[0010] Step 5: Signal quality and stability evaluation; Evaluate the quality of the signals processed by the core trajectory planning model, including indicators such as the signal-to-noise ratio and stability of the signals; if the signal quality meets the set standards, proceed to the next step; otherwise, return to Step 3 and continue to process the signals;

[0011] Step 6: Real-time trajectory planning and adjustment; According to the optimized trajectory planning signals, generate the motion trajectory of the robot in real time; at the same time, dynamically adjust the trajectory planning parameters according to environmental changes and the state of the robot to ensure that the robot can execute tasks stably and accurately.

[0012] As a further improvement of the present invention, the process of signal preprocessing and feature extraction in Step 2 is expressed as:

[0013] Step 2.1, Signal preprocessing

[0014] Let the signal sequence with noise collected be x(n), where n represents the serial number of discrete sampling points. Perform mean filtering on it, with the window size being M, and M being an odd number. The filtering formula is:

[0015]

[0016] where y(n) is the value of the signal sequence obtained after mean filtering and denoising at the nth point, and i is the filtering variable;

[0017] Step 2.2, Wavelet transform:

[0018] For the discrete-time signal x(n), its continuous wavelet transform is defined as:

[0019]

[0020] where W(a,b) is the feature of the signal at the corresponding time-frequency position, a is the scale factor, b is the translation factor, and ψ(n) is the wavelet basis function, denote its conjugate function;

[0021] Step 2.3, Fourier transform:

[0022] For the discrete signal x(n), its discrete Fourier transform can be expressed as:

[0023]

[0024] where j is the imaginary unit, N represents the length of the discrete signal x(n), and X(k) is the discrete Fourier transform feature of the signal x(n). By analyzing the amplitude magnitude, frequency distribution, etc. of X(k), the main frequency components contained in the signal can be understood.

[0025] As a further improvement of the present invention, the decision tree for identifying the interference source in Step 3 is represented as follows:

[0026] The decision tree analyzes the signal features in the training data set to construct a tree structure. Each internal node represents a test on a signal feature attribute, the branches represent different output results of the test, and the leaf nodes correspond to the final classification categories, that is, distinguishing useful signals and interference signals;

[0027] Step 3.1.1, Calculate the empirical entropy of the data set

[0028] Make the data processed in Step 2 into a training data set D, which contains m signal feature samples after preprocessing and feature extraction, and there are k categories in total, k = 2, namely the useful signal category and the interference signal category. Let the number of samples in the i-th category be m i ;

[0029] The empirical entropy H(D) is used to measure the degree of chaos of the data set D, and its calculation formula is as follows:

[0030]

[0031] Step 3.1.2, Calculate the information gain of the feature

[0032] For each signal feature A that can be used for partitioning, assume that its possible values are n, which are respectively denoted as a 1 , a 2 , …, a n ; According to the feature values of the feature A, the data set D is divided into n subsets D 1 , D 2 , …, D n , where D j contains the samples with the value of a j on the feature A, and |D j | represents the number of samples in the subset D j ;

[0033] The calculation formula for the information gain g(D, A) of feature A with respect to dataset D is as follows:

[0034]

[0035] where |D| represents the number of samples in dataset D, and H(D j ) represents the empirical entropy of dataset D j ;

[0036] Step 3.1.3, Select the optimal partitioning feature

[0037] During the process of constructing a decision tree, it is necessary to select the feature with the largest information gain from all available signal features as the partitioning attribute of the current node; traverse all features, calculate their respective information gains, and then select the feature with the largest information gain value;

[0038] Step 3.1.4, Recursively construct the decision tree

[0039] Using the selected optimal partitioning signal feature as the node, after partitioning the dataset according to different values (a 1 , a 2 , a 3 ) of the signal feature, for each subset (D 1 , D 2 , D 3 ), repeat the above steps of calculating the empirical entropy, information gain, and selecting the optimal partitioning feature, and continuously recursively construct the decision tree until the stopping condition is met; the stopping condition is: all samples belong to the same category, that is, the signal features corresponding to the samples within the subset have all been determined to be useful signals or interference signals;

[0040] Step 3.1.5, Use the constructed decision tree for interference source identification

[0041] When the construction of the decision tree is completed, for the newly collected signal feature vector x after preprocessing and feature extraction, starting from the root node of the decision tree, make judgments according to the corresponding feature attribute test conditions on the node, and follow the corresponding branches downward according to the test results, and finally reach the leaf node. The category corresponding to the leaf node is the category to which the signal feature vector x is identified, so as to distinguish whether it is a useful signal or an interference signal.

[0042] As a further improvement of the present invention, the spatial domain weighted mean filtering in Step 3 is expressed as follows:

[0043] Step 3.2.1, Determine the filtering object;

[0044] Take the part determined by the decision tree to contain interference signals as the processing object of the spatial domain weighted mean filtering, and represent the signal in the form of a two-dimensional matrix;

[0045] Step 3.2.2, construct the weight matrix and set the neighborhood range;

[0046] According to the spatial distribution characteristics of the signal and the expectation of the filtering effect, determine the neighborhood radii a and b in the horizontal and vertical directions to clarify the signal neighborhood range involved in the filtering calculation; meanwhile, construct an appropriate weight matrix W mn , where m = -a, …, a; n = -b, …, b, and use the Gaussian weighting function to construct the weight matrix, and its formula is:

[0047]

[0048] where σ is the standard deviation of the Gaussian function, which determines the distribution of the weights within the neighborhood and the attenuation rate;

[0049] Step 3.2.3, perform the filtering calculation;

[0050] For the signal at each position (i, j) in the signal matrix, where i represents the row index and j represents the column index, calculate the filtered signal value F according to the calculation formula of the spatial domain weighted mean filtering ij :

[0051]

[0052] where S i+m,j+n represents the original signal value at the corresponding position within the neighborhood.

[0053] As a further improvement of the present invention, the real-time trajectory planning and adjustment in step 6 are represented as follows:

[0054] Step 6.1 Real-time trajectory generation;

[0055] Generate the motion trajectory of the robot from the trajectory planning signal optimized after being processed through steps such as signal acquisition, preprocessing, interference source identification and filtering, trajectory planning model construction, and signal quality evaluation;

[0056] The optimized trajectory planning signal is represented by the mathematical function S(t), where t represents time, and S is a vector function containing multi-dimensional information such as position and velocity, S(t) = [x(t), y(t), v(t)], where x(t) represents the position information of the robot changing with time in the x-axis direction, y(t) is the position information in the y-axis direction, and v(t) is the velocity information;

[0057] For the motion trajectory planning of the robot in the plane, adopt the path point-based planning method, and given a series of discrete path points P 1 , P 2 , …, P n , each path point Pi Represent P in coordinate form i =(x i , y i ), i = 1, 2, …, n, and fit a continuous trajectory curve through an interpolation algorithm; between two adjacent path points P i (x i , y i ) and P i+1 (x i+1 , y i+1 ), when interpolating, for any moment t within the time interval [t i , t i+1 , its position coordinates (x(t), y(t)) can be calculated by the following formula:

[0058]

[0059] Obtain the continuous trajectory of the robot during the entire motion process through interpolation processing, so that it moves along the planned path;

[0060] Step 6.2 Trajectory dynamic adjustment;

[0061] It is detected by the sensor that a new obstacle appears at a distance d in front of the originally planned trajectory, and its position coordinates are (x obs , y obs ), while the current position coordinates of the robot are (x cur , y cur ), and the moving speed is v cur ; in order to avoid the obstacle, the artificial potential field method obstacle avoidance algorithm is used for adjustment; in the artificial potential field method, the target point generates an attractive force on the robot, and the obstacle generates a repulsive force on the robot. The resultant force F received by the robot can be expressed as the vector sum of the attractive force F att and the repulsive force F rep , that is:

[0062] F = F att+ F rep

[0063] The attractive force F att is related to the distance between the robot and the target point:

[0064] F att = k att (x gaol - x cur , y gaol - y cur )

[0065] Among them, k att is the attractive force coefficient, (x gaol , y gaol ) are the target point coordinates; the repulsive force Frep It is related to the distance between the robot and the obstacle. When the distance is less than a certain threshold d 0 a repulsive force is generated:

[0066]

[0067] where k rep is the repulsive force coefficient;

[0068] Adjust the movement direction of the robot according to the direction of the resultant force, and change the coordinates of the next path point. The calculation formula for the next path point (x new , y new ) is as follows:

[0069] x new = x cur + vcosθΔt

[0070] y new = y cur + vsinθΔt

[0071] where θ is the angle between the direction of the resultant force and the positive x-axis, Δt is the time interval, and v is the movement speed of the robot.

[0072] The time-frequency domain trajectory planning signal processing method for the autonomous navigation multi-sensor robot of the present invention has the following beneficial effects: The technical effects of the present invention are as follows:

[0073] 1. Through the collaborative work of multiple sensors, the present invention can obtain environmental information comprehensively and with high precision, avoiding the limitations of a single sensor, making the robot's perception of the environment more comprehensive and accurate, and providing a solid foundation for subsequent decision-making and actions.

[0074] 2. In terms of signal processing, the present invention adopts advanced preprocessing and feature extraction methods, effectively removing noise and interference, while maximizing the retention of useful signals, improving the quality and usability of the signals, and thus making the subsequent trajectory planning more accurate and reliable.

[0075] 3. The advanced trajectory planning model constructed by the present invention can adapt to complex and changing environments, generate and dynamically adjust the movement trajectory of the robot in real time, improve the flexibility and adaptability of the robot, enabling it to more efficiently avoid obstacles and accurately reach the target position. BRIEF DESCRIPTION OF THE DRAWINGS

[0076] Figure 1 is the flowchart of the present invention.

[0077] Figure 2 is the path planning schematic diagram of the present invention. DETAILED DESCRIPTION OF THE INVENTION

[0078] The present invention will be further described in detail below in conjunction with the accompanying drawings and specific embodiments:

[0079] The present invention relates to multi-sensor robot technology, including environmental perception and signal acquisition, preprocessing and feature extraction, interference recognition and filtering, trajectory planning construction, and evaluation and adjustment. Multiple sensors cooperate, advanced algorithms are used for processing, trajectory planning is optimized, signal quality is improved, and the adaptability and stability of the robot are enhanced to meet the requirements of complex tasks. The flow chart of the invention is as Figure 1 shown. The steps of the present invention will be introduced in detail below.

[0080] Step 1: Environmental perception and signal acquisition of the multi-sensor robot. The robot is equipped with a variety of sensors such as a high-definition camera, an ultrasonic sensor, and an infrared sensor to work together, continuously monitor the surrounding environment, and collect relevant time-frequency domain signal data.

[0081] The robot is simultaneously equipped with a variety of different types of sensors, including: a high-definition camera, an ultrasonic sensor, and an infrared sensor;

[0082] Different types of sensors cooperate to complete the tasks of environmental perception and signal acquisition. The high-definition camera captures the visual image information of the surrounding environment to present the appearance features of objects and scenes in the environment. The ultrasonic sensor uses the reflection characteristics of ultrasonic waves to detect distance information and calculates the distance between the robot and the object. The infrared sensor senses the infrared rays emitted by objects in the environment to judge the presence of objects and their relative positions, etc., and helps the robot detect whether there are heat sources or object obstructions around.

[0083] During the movement or task execution of the robot, these equipped sensors continuously monitor the surrounding environment. As the robot moves, the environmental conditions change continuously, and the sensors also track these changes in real time. When the robot enters a new area, the camera will update the captured images of the newly appeared objects in that area, the ultrasonic sensor will update the distance data according to the new distance situation, and the infrared sensor will correspondingly sense the heat source distribution in the new environment, etc., to ensure that the robot always grasps the latest environmental state.

[0084] While monitoring the environment, time-frequency domain signal data will be collected, which will become the basic data for the entire process analysis and processing. During the continuous transmission and reception of ultrasonic waves by the ultrasonic sensor, information such as the timestamps corresponding to each transmission and reception are recorded at regular time intervals. These data arranged in chronological order constitute the time-domain signal data, which can intuitively reflect the change of the object's distance over time; the high-definition camera will also record continuous image frame data according to time parameters such as the frame rate, reflecting the change process of the environmental visual information over time. When there are some periodically changing interference factors in the environment or the object itself has vibrations with specific frequencies, etc., the collected original time-domain signal is processed by an algorithm to obtain the distribution of the signal at different frequencies, that is, the frequency-domain signal data, which is used to further analyze some periodic characteristics and frequency-related characteristics hidden in the environment, providing more comprehensive data support for subsequent signal processing and trajectory planning, etc.

[0085] Step 2: Signal preprocessing and feature extraction. Preprocess the collected signals, including operations such as denoising and filtering, and then use methods such as wavelet analysis and Fourier transform to extract the time-frequency features of the signals, providing data support for subsequent trajectory planning.

[0086] Step 2.1, Signal preprocessing

[0087] Let the collected signal sequence with noise be x(n), where n represents the discrete sampling point serial number. Perform mean filtering on it, with the window size being M, and M being an odd number. The filtering formula is:

[0088]

[0089] Among them, y(n) is the value of the signal sequence obtained after mean filtering and denoising at the nth point, and i is the filtering variable.

[0090] Step 2.2, Wavelet transform:

[0091] For the discrete-time signal x(n), its continuous wavelet transform is defined as:

[0092]

[0093] Among them, W(a,b) is the feature of the signal at the corresponding time-frequency position, a is the scale factor, b is the translation factor, ψ(n) is the wavelet basis function, represents its conjugate function.

[0094] Step 2.3, Fourier transform:

[0095] For the discrete signal x(n), its discrete Fourier transform can be expressed as:

[0096]

[0097] where \(j\) is the imaginary unit, \(N\) represents the length of the discrete signal \(x(n)\), and \(X(k)\) is the discrete Fourier transform feature of the signal \(x(n)\). By analyzing the amplitude and frequency distribution of \(X(k)\), etc., the main frequency components contained in the signal can be understood.

[0098] Step 3: Interference source identification and noise filtering. Use a decision tree to identify the extracted signal features, distinguish useful signals from interference signals, and then use spatial filtering technology to filter out the interference signals to improve the signal quality.

[0099] Step 3.1: Interference source identification using the decision tree

[0100] By analyzing the signal features in the training dataset, the decision tree constructs a tree structure. Each internal node represents a test on a signal feature attribute, the branches represent different output results of the test, and the leaf nodes correspond to the final classification categories, that is, distinguishing useful signals from interference signals.

[0101] Step 3.1.1, Calculate the empirical entropy of the dataset

[0102] Make the data processed in Step 2 into a training dataset \(D\), which contains \(m\) signal feature samples after preprocessing and feature extraction, and there are \(k\) categories in total, \(k = 2\), namely the useful signal category and the interference signal category. Let the number of samples in the \(i\)-th category be \(m i .

[0103] The empirical entropy \(H(D)\) is used to measure the degree of chaos of the dataset \(D\), and its calculation formula is as follows:

[0104]

[0105] Step 3.1.2, Calculate the information gain of the feature

[0106] For each signal feature \(A\) that can be used for partitioning, assume that it may have \(n\) values, denoted as \(a 1 , a 2 , …, a n . According to the feature values of feature \(A\), the dataset \(D\) is divided into \(n\) subsets \(D 1 , D 2 , …, D n , where \(D j contains the samples with the value of \(a j on feature \(A\), and \(|D j |\) represents the number of samples in subset \(D j .

[0107] The calculation formula for the information gain \(g(D, A)\) of feature \(A\) with respect to dataset \(D\) is:

[0108]

[0109] Among them, |D| represents the number of samples in the dataset D, and H(D j ) represents the empirical entropy of the dataset D j .

[0110] Step 3.1.3, Select the optimal partitioning feature

[0111] In the process of constructing a decision tree, it is necessary to select the feature with the largest information gain from all available signal features as the partitioning attribute of the current node. Traverse all features, calculate their respective information gains, and then select the feature with the largest information gain value.

[0112] Step 3.1.4, Recursively construct the decision tree

[0113] Using the selected optimal partitioning signal feature as the node, after partitioning the dataset according to different values (a 1 , a 2 , a 3 ) of the signal feature, for each subset (D 1 , D 2 , D 3 ), repeat the above steps of calculating the empirical entropy, information gain, and selecting the optimal partitioning feature, and continuously recursively construct the decision tree until the stopping condition is met. The stopping condition is: all samples belong to the same category, that is, the signal features corresponding to the samples within the subset are all determined to be useful signals or interference signals.

[0114] Step 3.1.5, Use the constructed decision tree for interference source identification

[0115] When the construction of the decision tree is completed, for the newly collected signal feature vector x after preprocessing and feature extraction, starting from the root node of the decision tree, make judgments according to the corresponding feature attribute test conditions on the node, and follow the corresponding branches downward according to the test results, and finally reach the leaf node. The category corresponding to the leaf node is the category that the signal feature vector x is identified as, so as to distinguish whether it is a useful signal or an interference signal.

[0116] Step 3.2, Spatial domain weighted mean filtering

[0117] Step 3.2.1, Determine the filtering object

[0118] Take the part determined by the decision tree to contain interference signals as the processing object of spatial domain weighted mean filtering, and represent the signal in the form of a two-dimensional matrix.

[0119] Step 3.2.2, Construct the weight matrix and set the neighborhood range

[0120] According to the spatial distribution characteristics of the signal and the expectation of the filtering effect, determine the neighborhood radii a and b in the horizontal and vertical directions, thereby defining the signal neighborhood range involved in the filtering calculation. Meanwhile, construct an appropriate weight matrix W mn , where m = -a, …, a; n = -b, …, b, and use the Gaussian weighting function to construct the weight matrix, and its formula is:

[0121]

[0122] where σ is the standard deviation of the Gaussian function, which determines the distribution of weights within the neighborhood and the attenuation rate.

[0123] Step 3.2.3, perform the filtering calculation

[0124] For the signal at each position (i, j) in the signal matrix, where i represents the row index and j represents the column index, calculate the filtered signal value F according to the calculation formula of spatial domain weighted mean filtering ij :

[0125]

[0126] where S i+m,j+n represents the original signal value at the corresponding position within the neighborhood.

[0127] Step 4: Construct the trajectory planning model. According to the signal after removing interference, construct a trajectory planning model for curve fitting collaborative operation, and use a high-precision curve fitting algorithm to optimize the trajectory planning signal.

[0128] Calculate the change rate of the distance between adjacent time points using ultrasonic and infrared data, which reflects the relative motion speed between the robot and the obstacle; for infrared data, extract the temperature gradient information to sense the dynamic changes in the temperature or the approach of objects in the environment. The features extracted from the camera images include the contour of the object and the coordinates of the feature points. By matching and tracking the feature points in adjacent frame images, calculate the displacement of the feature points, and then obtain the motion speed and direction of the object relative to the robot.

[0129] Step 4.1, polynomial fitting, determine the order n according to the complexity of the robot motion trajectory and perform polynomial fitting:

[0130] p(t) = c 0 + c 1 t + … + c n t n

[0131] where p(t) is the polynomial function of the robot trajectory, c 0 , c 1 ,..., c nare the coefficients of the polynomial, and t is the time variable.

[0132] Step 4.2, Select fitting points: Select different time instants t 1 , t 2 , …, t m corresponding trajectory data r(t 1 ), r(t 2 ), …, r(t m ) to cover the entire motion process. Let the coefficient vector c = (c 0 , c 1 ,..., c n ), and construct the objective function S(c) for curve fitting:

[0133]

[0134] Take the partial derivative to obtain the system of equations:

[0135]

[0136] where k = 0, 1, …, n, and solve for c 0 , c 1 ,..., c n .

[0137] Step 4.3, Information feedback: Feed the trajectory r'(x) after spatial filtering back to the trajectory planning model, judge anomalies by calculating the difference between r'(x) and the original trajectory r(x), and adjust the parameters c 0 , c 1 ,..., c n . After judging anomalies, substitute into p(t) to recalculate c 0 , c 1 ,..., c n to optimize the trajectory.

[0138] Step 5: Signal quality and stability evaluation. Evaluate the quality of the signal processed by the trajectory planning core model, including indicators such as the signal-to-noise ratio and stability of the signal. If the signal quality meets the set standards, proceed to the next step; otherwise, return to Step 3 and continue to process the signal.

[0139] Step 6: Real-time trajectory planning and adjustment. According to the optimized trajectory planning signal, generate the motion trajectory of the robot in real time. At the same time, dynamically adjust the trajectory planning parameters according to environmental changes and the robot's state to ensure that the robot can execute tasks stably and accurately.

[0140] Step 6.1 Real-time trajectory generation

[0141] The trajectory planning signal optimized after processing steps such as signal acquisition, preprocessing, interference source identification and filtering, trajectory planning model construction, and signal quality assessment is used to generate the motion trajectory of the robot. The path planning is shown as Figure 2 shown below.

[0142] The optimized trajectory planning signal is represented by the mathematical function S(t), where t represents time and S is a vector function containing multi-dimensional information such as position and velocity. S(t) = [x(t), y(t), v(t)], where x(t) represents the position information of the robot changing with time in the x-axis direction, y(t) is the position information in the y-axis direction, and v(t) is the velocity information.

[0143] For the motion trajectory planning of the robot in the plane, a path point-based planning method is adopted. A series of discrete path points P 1 , P 2 , …, P n are given. Each path point P i is represented in coordinate form as P i = (x i , y i ), i = 1, 2, …, n. A continuous trajectory curve is fitted through an interpolation algorithm. When interpolating between two adjacent path points P i (x i , y i ) and P i+1 (x i+1 , y i+1 ), for any moment t within the time interval [t i , t i+1 , its position coordinates (x(t), y(t)) can be calculated by the following formula:

[0144]

[0145] The continuous trajectory of the robot during the entire motion process is obtained through interpolation processing, enabling it to move along the planned path.

[0146] Step 6.2 Trajectory Dynamic Adjustment

[0147] It is detected by the sensor that a new obstacle appears at a distance d in front of the originally planned trajectory, with the position coordinates (x obs , y obs ), while the current position coordinates of the robot are (x cur , y cur ), and the motion speed is v cur. To avoid obstacles, the artificial potential field method obstacle avoidance algorithm is adopted for adjustment. In the artificial potential field method, the target point generates an attractive force on the robot, and the obstacle generates a repulsive force on the robot. The resultant force F received by the robot can be expressed as the vector sum of the attractive force F att and the repulsive force F rep , that is:

[0148] F = F att+ F rep

[0149] The attractive force F att is related to the distance between the robot and the target point:

[0150] F att = k att (x gaol - x cur , y gaol - y cur )

[0151] where k att is the attractive force coefficient, (x gaol , y gaol ) is the target point coordinate; the repulsive force F rep is related to the distance between the robot and the obstacle. When the distance is less than a certain threshold d 0 , a repulsive force is generated:

[0152]

[0153] where k rep is the repulsive force coefficient.

[0154] According to the direction of the resultant force, the movement direction of the robot is adjusted, and the coordinates of the next path point are changed. The calculation formula for the next path point (x new , y new ) is as follows:

[0155] x new = x cur + vcosθΔt

[0156] y new = y cur + vsinθΔt

[0157] where θ is the angle between the direction of the resultant force and the positive direction of the x-axis, Δt is the time interval, and v is the movement speed of the robot.

[0158] The above is only a preferred embodiment of the present invention, and it is not a limitation of the present invention in any other form. Any modification or equivalent change made according to the technical essence of the present invention still belongs to the scope protected by the present invention.

Claims

1. A signal processing method for time-frequency domain trajectory planning of an autonomous navigation multi-sensor robot, the specific steps are as follows, and the characteristics are as follows: Step 1: Multi-sensor robot environmental perception and signal acquisition: The robot is equipped with a variety of sensors such as high-definition cameras, ultrasonic sensors, infrared sensors, etc. to work together to monitor the surrounding environment in real time and collect relevant time-frequency domain signal data; Step 2: Signal preprocessing and feature extraction: Preprocess the collected signals, including denoising, filtering and other operations, and then use wavelet analysis, Fourier transform and other methods to extract the time-frequency characteristics of the signals to provide data support for subsequent trajectory planning; Step 3: Interference source identification and noise filtering: Use decision trees to identify the extracted signal features and distinguish useful signals from interference signals; then use spatial filtering technology to filter out interference signals and improve signal quality; Step 4: Trajectory planning model construction: Based on the signal after interference is filtered out, a trajectory planning model with curve fitting collaboration is constructed, and the trajectory planning signal is optimized using a high-precision curve fitting algorithm; Step 5: Signal quality and stability assessment: perform quality assessment on the signal after being processed by the trajectory planning core model, including the signal-to-noise ratio, stability and other indicators; if the signal quality meets the set standard, proceed to the next step; otherwise, return to step 3 and continue processing the signal; Step 6: Real-time trajectory planning and adjustment: Generate the robot's motion trajectory in real time based on the optimized trajectory planning signal; at the same time, dynamically adjust the trajectory planning parameters according to environmental changes and robot status to ensure that the robot can perform tasks stably and accurately.

2. The signal processing method for time-frequency domain trajectory planning of an autonomous navigation multi-sensor robot according to claim 1 is characterized in that: The process of signal preprocessing and feature extraction in step 2 is expressed as: Step 2.1, signal preprocessing Assume that the collected signal sequence containing noise is x(n), where n represents the discrete sampling point number, and perform mean filtering on it. The window size is M, where M is an odd number, and the filtering formula is: Among them, y(n) is the value of the signal sequence at point n after mean filtering and denoising, and i is the filtering variable; Step 2.2, wavelet transform: For a discrete-time signal x(n), its continuous wavelet transform is defined as: Among them, W(a,b) is the characteristic of the signal at the corresponding time-frequency position, a is the scale factor, b is the translation factor, ψ(n) is the wavelet basis function, represents its conjugate function; Step 2.3, Fourier transform: For a discrete signal x(n), its discrete Fourier transform can be expressed as: Where j is an imaginary unit, N represents the length of the discrete signal x(n), and X(k) is the discrete Fourier transform characteristic of the signal x(n). By analyzing the amplitude and frequency distribution of X(k), we can understand the main frequency components contained in the signal.

3. The signal processing method for time-frequency domain trajectory planning of an autonomous navigation multi-sensor robot according to claim 1, characterized in that: The decision tree for identifying interference sources in step 3 is expressed as follows: The decision tree constructs a tree structure by analyzing the signal features in the training data set. Each internal node represents a test of a signal feature attribute, the branch represents the different output results of the test, and the leaf node corresponds to the final classification category, that is, distinguishing between useful signals and interference signals. Step 3.1.1, calculate the empirical entropy of the data set The data processed in step 2 is made into a training data set D, which contains m signal feature samples after preprocessing and feature extraction. There are k categories in total, k = 2, namely the useful signal category and the interference signal category. Let the number of samples in the i-th category be m i ; The empirical entropy H(D) is used to measure the degree of confusion of the data set D. Its calculation formula is as follows: Step 3.1.2, calculate the information gain of the feature For each signal feature A that can be used for division, suppose there are n possible values, which are recorded as a1, a2, ..., a n ; According to the feature value of feature A, the data set D is divided into n subsets D1, D2, ..., D n , where D j Contains the value a on feature A j of samples, and |D j | represents subset D j The number of samples; The information gain g(D,A) of feature A to data set D is calculated as: Among them, |D| represents the number of samples in the data set D, H(D j ) represents the data set D j The empirical entropy of Step 3.1.3, select the optimal segmentation feature In the process of building a decision tree, it is necessary to select the feature with the largest information gain from all available signal features as the partition attribute of the current node; traverse all features, calculate their respective information gains, and then select the feature with the largest information gain value; Step 3.1.4, recursively build a decision tree Taking the selected optimal partitioning signal feature as the node, after partitioning the data set according to different values ​​of the signal feature (a1, a2, a3), repeat the above steps of calculating the empirical entropy, information gain and selecting the optimal partitioning feature for each subset (D1, D2, D3), and continuously recursively construct the decision tree until the stopping condition is met; the stopping condition is: all samples belong to the same category, that is, the signal features corresponding to the samples in the subset are all judged to be useful signals or interference signals; Step 3.1.5: Use the constructed decision tree to identify interference sources After the construction of the decision tree is completed, for the newly collected signal feature vector x after preprocessing and feature extraction, start from the root node of the decision tree, judge according to the corresponding feature attribute test conditions on the node, go down along the corresponding branch according to the test results, and finally reach the leaf node. The category corresponding to the leaf node is the category that the signal feature vector x is identified as, thereby distinguishing whether it is a useful signal or an interference signal.

4. The signal processing method for time-frequency domain trajectory planning of an autonomous navigation multi-sensor robot according to claim 1, characterized in that: The spatial domain weighted mean filtering in step 3 is expressed as follows: Step 3.2.1, determine the filtering object; The part determined by the decision tree as containing interference signals is used as the processing object of spatial domain weighted mean filtering, and the signal is represented in the form of a two-dimensional matrix; Step 3.2.2, construct the weight matrix and set the neighborhood range; According to the spatial distribution characteristics of the signal and the expectation of the filtering effect, the neighborhood radius a and b in the horizontal and vertical directions are determined to clarify the signal neighborhood range involved in the filtering calculation; at the same time, a suitable weight matrix W is constructed. mn , where m = -a,…,a; n = -b,…,b, and the Gaussian weighting function is used to construct the weight matrix, and its formula is: Among them, σ is the standard deviation of the Gaussian function, which determines the distribution of weights in the neighborhood and the decay rate; Step 3.2.3, perform filtering calculation; For each signal at position (i, j) in the signal matrix, where i represents the row index and j represents the column index, the filtered signal value F is calculated according to the calculation formula of the spatial domain weighted mean filter. ij : Among them, S i+m,j+n Represents the original signal value of the corresponding position in the neighborhood.

5. The signal processing method for time-frequency domain trajectory planning of an autonomous navigation multi-sensor robot according to claim 1, characterized in that: The real-time trajectory planning and adjustment in step 6 are shown as follows: Step 6.1 Real-time trajectory generation; After signal acquisition, preprocessing, interference source identification and filtering, trajectory planning model construction, and signal quality evaluation, the trajectory planning signal is optimized to generate the robot's motion trajectory; The optimized trajectory planning signal is expressed as a mathematical function S(t), where t represents time, S is a vector function containing multi-dimensional information such as position and speed, S(t) = [x(t), y(t), v(t)], where x(t) represents the position information of the robot in the x-axis direction over time, y(t) is the position information in the y-axis direction, and v(t) is the speed information; For the motion trajectory planning of the robot in the plane, a planning method based on path points is adopted. Given a series of discrete path points P1, P2, ..., P n , each path point P i Express P in coordinate form i =(x i ,y i ), i = 1, 2, ..., n, a continuous trajectory curve is fitted by the interpolation algorithm; at two adjacent path points P i (x i ,y i ) and P i+1 (x i+1 ,y i+1 ), for the time interval [t i ,t i+1 ], its position coordinates (x(t), y(t)) can be calculated by the following formula: The continuous trajectory of the robot during the entire movement process is obtained through interpolation processing, so that it moves according to the planned path; Step 6.2 trajectory dynamic adjustment; The sensor detects that a new obstacle appears at a distance d ahead of the originally planned trajectory, and its position coordinates are (x obs ,y obs ), and the robot’s current position coordinates are (x cur ,y cur ), the moving speed is v cur ; In order to avoid obstacles, the artificial potential field method obstacle avoidance algorithm is used for adjustment; in the artificial potential field method, the target point generates gravitational force on the robot, and the obstacle generates repulsive force on the robot. The resultant force F on the robot can be expressed as gravitational force F att and repulsive force F rep The vector sum of , that is: F=F att+ F rep Gravity F att Related to the distance from the robot to the target point: F att =k att (x gaol -x cur ,y gaol -y cur ) Among them, k att is the gravitational coefficient, (x gaol ,y gaol ) is the target point coordinate; repulsive force F rep It is related to the distance between the robot and the obstacle. When the distance is less than a certain threshold d0, repulsion is generated: Among them, k rep is the repulsion coefficient; According to the direction of the resultant force, the robot's movement direction is adjusted to change the coordinates of the next path point. new ,y new ) is calculated as follows: x new =x cur +vcosθΔt y new =y cur +vsinθΔt Among them, θ is the angle between the direction of the resultant force and the positive direction of the x-axis, Δt is the time interval, and v is the robot's movement speed.

Images