Robot cleaning control and path planning method in harsh environment

Abstract

This invention relates to the field of general control and regulation systems, specifically to a method for robot cleaning control and path planning in harsh environments. The method acquires real-time negative pressure values ​​and drive wheel speed signals, converts the real-time negative pressure values ​​into fan load torque variables, and incorporates them into the chassis dynamics equations to calculate the chassis slip ratio. When the slip ratio exceeds a threshold, the negative pressure change rate is extracted and combined with historical pose data to construct a dust concentration gradient field. The chassis slip ratio and dust concentration gradient field are fused to generate a comprehensive cost map. A travel trajectory is generated with the constraint of reducing slip ratio costs and moving along the gradient descent direction. The drive wheel torque and fan power are adjusted based on the trajectory curvature. This invention incorporates cleaning load characteristics into chassis anti-slip control and path planning, avoiding slip areas, ensuring path tracking accuracy and cleaning operation continuity in high-dust environments, and reducing the risk of drive motor overload.

Description

Technical Field

[0001] This invention relates to the field of general control and regulation system technology, and specifically to a method for robot cleaning control and path planning in harsh environments. Background Technology

[0002] In harsh, high-dust environments, autonomous cleaning robots typically employ a control architecture where the environmental perception module and the cleaning execution module are independent. In existing conventional solutions, the robot relies on external sensors to acquire spatial geometric information to construct an occupation grid map. A path planning algorithm then generates a covering trajectory based on the obstacle positions and boundary constraints within this map. During movement, the chassis control system uses encoder feedback from the drive wheel motors to perform a speed closed loop, maintaining the robot at a preset linear velocity. Regarding cleaning operations, the operation of the fan and brushes is managed by independent control loops, performing constant-power suction or gear switching only based on a pre-set fixed program or a trigger signal from a single dust concentration sensor. The load changes of the fan and brushes are not integrated into the chassis's dynamic control process.

[0003] When the aforementioned conventional solution operates on soft, dusty surfaces, the aerodynamic load torque of the sweeping fan fluctuates drastically as it continuously draws in large amounts of dust. This load fluctuation is directly transmitted to the robot chassis, altering the adhesion between the drive wheels and the ground. Because the sweeping load and chassis dynamics are decoupled in the existing technology, the chassis speed closed-loop control cannot detect the traction attenuation caused by changes in the fan load. This results in the drive wheels continuously slipping on the dust layer, causing the robot to deviate from its intended trajectory or even come to a standstill, thus interrupting the sweeping and covering operation. Summary of the Invention

[0004] The purpose of this invention is to provide a robot cleaning control and path planning method for harsh environments, which can effectively solve the problems in the background art.

[0005] To achieve the above objectives, the technical solution adopted by the present invention is as follows:

[0006] A method for robot cleaning control and path planning in harsh environments, including: real-time acquisition of the robot's drive wheel speed signal and the real-time negative pressure value of the cleaning fan;

[0007] The real-time negative pressure value is converted into a fan load torque variable, and the fan load torque variable and the drive wheel speed signal are input into a preset chassis slip ratio calculation model to obtain the chassis slip ratio at the current moment.

[0008] When the chassis slip ratio is greater than the preset slip ratio threshold, the negative pressure change rate of the real-time negative pressure value at the current moment is extracted as the dust concentration feature, and a dust concentration gradient field with the current coordinate point as the starting point is constructed by combining the robot's historical pose data.

[0009] The chassis slip ratio is fused with the dust concentration gradient field to generate a comprehensive cost map;

[0010] The travel trajectory is generated by a path planning algorithm under the constraint of reducing the slip ratio cost in the comprehensive cost map and moving along the gradient descent direction of the dust concentration gradient field.

[0011] Based on the curvature of the travel trajectory, the torque of the robot's drive wheels and the power of the cleaning fan are adjusted.

[0012] Preferably, the real-time negative pressure value is converted into a fan load torque variable, and the fan load torque variable and the drive wheel speed signal are input to a preset chassis slip ratio calculation model to obtain the chassis slip ratio at the current moment, including: obtaining the current speed of the cleaning fan, and using the ratio of the real-time negative pressure value to the current speed as the aerodynamic drag coefficient;

[0013] The aerodynamic drag coefficient is multiplied by the square of the current rotational speed, and combined with the preset mechanical friction compensation coefficient, to calculate the fan load torque variable.

[0014] The drive wheel speed signal is converted into drive wheel linear velocity, and the fan load torque variable is introduced as a disturbance term into the robot's chassis dynamics equation. The theoretical drive wheel linear velocity is obtained by solving the chassis dynamics equation.

[0015] The chassis slip ratio at the current moment is obtained by dividing the difference between the theoretical drive wheel linear velocity and the drive wheel linear velocity by the theoretical drive wheel linear velocity.

[0016] Preferably, the negative pressure change rate of the real-time negative pressure value at the current moment is extracted as a dust concentration feature, and a dust concentration gradient field is constructed with the robot's historical pose data as the starting point, including: truncating the real-time negative pressure value with a fixed time window, and performing first-order difference calculation on the truncated negative pressure time series to obtain the negative pressure change rate;

[0017] Retrieve the coordinate sequence from the historical pose data corresponding to the fixed time window;

[0018] Each coordinate point in the coordinate sequence is taken as a spatial node, and the negative pressure change rate corresponding to each spatial node is mapped to the dust concentration estimate of the spatial node.

[0019] Centered on the current coordinate point, a preset spatial interpolation algorithm is used to interpolate the estimated dust concentration values ​​of adjacent spatial nodes to generate a dust concentration gradient field containing spatial distribution information.

[0020] Preferably, fusing the chassis slip ratio with the dust concentration gradient field to generate a comprehensive cost map includes: obtaining the dust concentration estimate corresponding to each grid cell in the dust concentration gradient field in a preset basic occupancy grid map;

[0021] The dust concentration estimate is input into a preset slip penalty function to calculate the slip penalty value for each grid cell, wherein the slope of the slip penalty function is positively correlated with the chassis slip ratio.

[0022] Obtain the basic passage cost of the free grid cells in the basic occupied grid map;

[0023] The basic passage cost value of each free grid cell is weighted and summed with the corresponding slip penalty cost value. The summation result is used as the comprehensive cost value of the free grid cell in the comprehensive cost map. The comprehensive cost value of the obstacle grid cell is set to infinity.

[0024] Preferably, the path planning algorithm generates a travel trajectory under the constraint of reducing the slip ratio cost in the comprehensive cost map and moving along the gradient descent direction of the dust concentration gradient field, including: taking the robot's current coordinate point as the starting node and determining the set of adjacent extended nodes of the starting node in the comprehensive cost map;

[0025] Calculate the direction vector of the line connecting the starting node to each node in the set of adjacent extended nodes;

[0026] Calculate the negative gradient direction vector of the starting node in the dust concentration gradient field;

[0027] Calculate the cosine of the angle between each of the connecting direction vectors and the negative gradient direction vector, and add the cosine of the angle to the comprehensive cost value of the nodes in the adjacent extended node set to obtain the evaluation cost value;

[0028] The node with the lowest evaluation value is selected as the target child node, and the expansion process is repeated until the termination condition is met. The path connecting all target child nodes is taken as the travel trajectory.

[0029] Preferably, adjusting the drive wheel torque of the robot and the fan power of the cleaning fan according to the trajectory curvature of the travel trajectory includes: discretizing the travel trajectory to obtain a sequence of trajectory points composed of multiple discrete trajectory points, and calculating the trajectory curvature corresponding to each discrete trajectory point;

[0030] When the trajectory curvature of the discrete trajectory point is greater than a preset curvature threshold, the current turning angular velocity of the robot is obtained, the torque adjustment amount is calculated based on the difference between the current turning angular velocity and the trajectory curvature, and the torque adjustment amount is superimposed on the reference value of the drive wheel torque.

[0031] The estimated centrifugal acceleration on the side where the cleaning fan is located is calculated based on the trajectory curvature. The product of the fan power and the estimated centrifugal acceleration is used as the power suppression parameter. The initial power of the cleaning fan is subtracted from the power suppression parameter to obtain the final output power.

[0032] Preferably, the aerodynamic drag coefficient is multiplied by the square of the current rotational speed, and combined with a preset mechanical friction compensation coefficient to calculate the fan load torque variable, including: real-time acquisition of the three-phase current signal of the fan drive motor driving the cleaning fan, performing Clark transformation and Park transformation on the three-phase current signal, and extracting the d-axis current component and q-axis current component.

[0033] Multiply the q-axis current component by the torque constant of the cleaning fan to obtain the measured output torque;

[0034] The theoretical aerodynamic load torque is obtained by multiplying the aerodynamic drag coefficient by the square of the current rotational speed.

[0035] The difference between the measured output torque and the theoretical aerodynamic load torque is used as the dynamic friction torque observation value. The dynamic friction torque observation value is filtered using a low-pass filter, and the filtering result is used as the updated mechanical friction compensation coefficient in the calculation.

[0036] Preferably, each coordinate point in the coordinate sequence is taken as a spatial node, and the negative pressure change rate corresponding to each spatial node is mapped to the dust concentration estimate of the spatial node, including: synchronously acquiring the brush rotation speed signal of the robot's cleaning brush within the fixed time window;

[0037] Extract the continuous high-speed periods in the brush rotation speed signal that are greater than a preset speed threshold, and mark the spatial nodes in the coordinate sequence corresponding to the continuous high-speed periods as effective disturbance nodes;

[0038] For the effective disturbance node, the ratio of the negative pressure change rate to the brush rotation speed signal at the corresponding time is calculated as the standardized concentration feature;

[0039] For non-effective perturbation nodes, the standardized concentration feature is reduced according to the time decay factor;

[0040] The standardized concentration characteristics of the effective perturbation nodes and the reduced characteristics of the ineffective perturbation nodes are used as the dust concentration estimates for the corresponding spatial nodes.

[0041] Preferably, the cosine value of the angle between each of the connecting direction vectors and the negative gradient direction vector is calculated, and the cosine value of the angle is added to the comprehensive cost value of the nodes in the adjacent extended node set to obtain the evaluation cost value, including: obtaining the chassis wheelbase and maximum steering angle of the robot, and calculating the minimum turning radius of the robot;

[0042] Determine whether the distance from the starting node to a node in the set of adjacent extended nodes is less than the minimum turning radius;

[0043] If it is less than the minimum turning radius, then the connecting direction vector is projected onto the tangent direction of the arc with the starting node as the center and the minimum turning radius as the radius to obtain the corrected direction vector;

[0044] Calculate the cosine of the angle between the corrected direction vector and the negative gradient direction vector, and add the reciprocal of the distance as a distance penalty term to the calculation of the evaluation cost value, replacing the original comprehensive cost value for addition calculation.

[0045] Preferably, the centrifugal acceleration estimate on the side where the cleaning fan is located is calculated based on the trajectory curvature, and the product of the fan power and the centrifugal acceleration estimate is used as the power suppression parameter, including: obtaining the installation offset distance and installation offset angle of the robot's cleaning fan in the vehicle coordinate system;

[0046] The current lateral acceleration of the robot is calculated based on the trajectory curvature of the discrete trajectory points and the robot's current travel velocity.

[0047] The current lateral acceleration is decomposed into the direction corresponding to the installation offset angle, and the estimated centrifugal acceleration of the sweeping fan is calculated by combining the installation offset distance.

[0048] The q-axis current component of the fan drive motor is retrieved. When the q-axis current component is greater than the preset overcurrent threshold, the power suppression parameter is multiplied by a forced attenuation coefficient greater than 1 and then subtracted from the initial power to limit the lower limit of the fan power to not be lower than the minimum power threshold for maintaining negative pressure.

[0049] Compared with the prior art, the beneficial effects of the present invention are as follows:

[0050] 1. This invention converts the real-time negative pressure value of the cleaning fan into a fan load torque variable, and introduces it as an interference term into the chassis dynamics equation to calculate the chassis slip ratio. Simultaneously, it extracts the negative pressure change rate and combines it with historical pose data to construct a dust concentration gradient field. The chassis slip ratio and dust concentration gradient field are then fused to generate a comprehensive cost map. In path planning, the trajectory is generated with the constraint of reducing the slip ratio cost and moving along the gradient descent direction. These methods directly transform the physical characteristics of the cleaning load into constraints for chassis anti-slip control and path planning, enabling the robot to avoid slippage areas caused by fan suction on soft, dusty surfaces and to move in accordance with dust distribution characteristics. This ensures the robot's path tracking accuracy and cleaning coverage continuity in harsh, high-dust environments.

[0051] 2. In this invention, when adjusting the drive wheel torque and fan power based on the trajectory curvature, if the trajectory curvature exceeds a threshold, the torque adjustment amount is calculated based on the difference between the steering angular velocity and the trajectory curvature. This is combined with an estimated centrifugal acceleration calculated from the fan's installation position as a power suppression parameter to reduce fan power. This method reduces the disturbance of fan load on chassis attitude during cornering. Furthermore, by utilizing the three-phase current signal of the fan drive motor to extract the measured output torque and dynamically correct the mechanical friction compensation coefficient, the accuracy of the fan load torque variable calculation is ensured, reducing the overload risk of the drive motor under complex operating conditions. Attached Figure Description

[0052] Figure 1 This is a flowchart of the overall method for robot cleaning control and path planning in harsh environments according to the present invention;

[0053] Figure 2 This is a flowchart of the chassis slip ratio calculation method of the present invention;

[0054] Figure 3 This is a flowchart of the dust concentration gradient field construction method of the present invention;

[0055] Figure 4 This is a flowchart of the comprehensive cost map generation method of the present invention;

[0056] Figure 5 A flowchart for generating a travel trajectory under the constraints of this invention;

[0057] Figure 6 This is a flowchart of the torque and power regulation method based on trajectory curvature of the present invention. Detailed Implementation

[0058] 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, not all, of the embodiments of the present invention. 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.

[0059] Please refer to Figure 1 This embodiment provides a robot cleaning control and path planning method for harsh environments. This implementation is applied to a wheeled autonomous cleaning robot in high-dust environments. The robot is equipped with a drive wheel encoder, a fan negative pressure sensor, a pose measurement unit, a chassis drive controller, a fan drive controller, and a path planning processor. The drive wheel encoder collects the drive wheel speed signal. The negative pressure sensor is installed inside the air inlet duct of the cleaning fan to collect the real-time negative pressure value inside the fan. The pose measurement unit uses a combined positioning module of a wheel odometer and an inertial measurement unit to output the robot's real-time pose data and historical pose data.

[0060] The robot's drive wheel speed signal and the real-time negative pressure value of the cleaning fan are acquired in real time. The drive wheel encoder collects the pulse signal of the drive wheel at a fixed sampling frequency and converts the pulse signal into a drive wheel speed signal. The sampling frequency range is 50Hz. 200Hz. The negative pressure sensor adopts a piezoresistive negative pressure sensor, and the sampling frequency is synchronized with the sampling frequency of the drive wheel encoder to ensure that the drive wheel speed signal collected at the same time has time alignment characteristics with the real-time negative pressure value, and the time alignment error does not exceed 1ms.

[0061] This invention employs a hardware synchronous trigger sampling architecture to achieve microsecond-level time alignment: the drive wheel encoder and the negative pressure sensor share the same synchronous trigger signal output from the advanced timer TIM1 of the STM32H7 series MCU. The trigger signal frequency is consistent with the sampling frequency, and the trigger edge accuracy is ≤100ns. Internally, the MCU writes the two sampling data streams into the same circular buffer via a DMA (Direct Memory Access) channel, and adds the same timestamp tag to each data set in the buffer. The timestamp is generated by a 32-bit high-precision timer (counting frequency 1MHz) inside the MCU, ensuring hardware-level time synchronization.

[0062] To ensure accuracy under vibration and electromagnetic interference environments, triple anti-interference measures are adopted:

[0063] Hardware filtering: A second-order RC low-pass filter (cutoff frequency 250Hz) is connected in series at the output of the negative pressure sensor signal, and differential signal transmission is used to suppress common-mode electromagnetic interference; the encoder signal uses opto-isolated input to block conducted interference in the motor drive circuit.

[0064] Software timestamp verification: The continuity of timestamps of the sampled data is verified in real time. If a timestamp jump of more than 1ms is detected, the abnormal data is discarded and the data is completed by linear interpolation of the two adjacent valid data sets.

[0065] Vibration compensation: Install rubber damping pads (natural frequency ≤10Hz) at the installation location of the negative pressure sensor to isolate the impact of robot chassis vibration on the sensor diaphragm and avoid sudden changes in sampling values ​​caused by vibration.

[0066] The acquired raw speed and negative pressure signals are preprocessed using a moving average filter with a sliding window length of a preset fixed number of sampling points to eliminate high-frequency noise interference during the sampling process.

[0067] The real-time negative pressure value is converted into a fan load torque variable, and the fan load torque variable and the drive wheel speed signal are input into a preset chassis slip ratio calculation model to obtain the chassis slip ratio at the current moment. The preset chassis slip ratio calculation model is constructed based on the robot chassis dynamics characteristics. The input of the model is the fan load torque variable and the drive wheel speed signal, and the output is the chassis slip ratio at the current moment. The aerodynamic load of the cleaning fan is directly related to the negative pressure value inside the fan. During the operation of the fan, the negative pressure value in the air inlet duct changes with the concentration of inhaled dust. The higher the dust concentration, the greater the air inlet resistance, the higher the negative pressure value inside the fan, and the greater the corresponding aerodynamic load torque. Beforehand, a mapping relationship between real-time negative pressure value and fan load torque variable is established through calibration experiments. During the calibration process, at the rated speed of the fan, the throttling opening of the air inlet pipe is adjusted sequentially to simulate the air inlet resistance under different dust concentrations. The real-time negative pressure value and the corresponding fan output torque under different openings are recorded to construct a mapping table. During real-time operation, the real-time negative pressure value at the current moment is converted into the corresponding fan load torque variable by interpolation through table lookup.

[0068] The drive wheel speed signal is converted into drive wheel linear velocity using the following formula:

[0069]

[0070] in, This represents the measured linear velocity of the drive wheel at the current moment. This is the current drive wheel speed signal; is the rolling radius of the drive wheel, and is a pre-calibrated fixed parameter.

[0071] The wind turbine load torque variable is introduced as a disturbance term into the robot's chassis dynamics equations. These equations consider the impact of the wind turbine load torque on the overall chassis traction. The wind turbine load torque is transmitted to the drive wheel axle system through the robot's rigid body structure, altering the normal load distribution between the drive wheels and the ground, thus affecting the maximum adhesion traction of the drive wheels. The expression for the chassis dynamics equations is:

[0072]

[0073] Where M is the total mass of the robot; This is the theoretical acceleration of the robot's body; This refers to the total traction force output by the drive wheels; The rolling resistance of the vehicle body; The interference resistance introduced by the wind turbine load torque.

[0074] Among them, the total traction force output by the drive wheel is related to the input torque of the drive wheel, and the rolling resistance is a pre-calibrated fixed parameter related to the vehicle body mass;

[0075] The calculation method for the total traction force output by the drive wheels is as follows: The total traction force output by the drive wheels is determined by both the input torque of the drive wheels and the efficiency of the transmission system. The calculation formula is:

[0076]

[0077] in, The output torque of the drive wheel motor at the current moment is obtained by multiplying the q-axis current component of the drive motor by the motor torque constant. , The torque constant of the drive motor, (This refers to the q-axis current component of the drive motor). The mechanical efficiency of the drive wheel transmission system is a pre-calibrated fixed parameter (ranging from 0.85 to 0.95). The radius of the driving wheel's rolling radius.

[0078] Calibration method for vehicle body rolling resistance: Rolling resistance is calibrated using a horizontal road surface sliding test: The robot is placed on a flat, hard horizontal road surface and accelerated to a preset speed. Then, the power to the drive motor is cut off, allowing the robot to glide freely until it stops. The speed-time curve during the glide is recorded by an encoder. According to Newton's second law, rolling resistance... satisfy:

[0079]

[0080] in, For the overall quality of the robot vehicle; The average deceleration during the gliding process is calculated from the slope of the velocity-time curve. The experiment was repeated 5 times, and the average value was taken as the final rolling resistance parameter. This parameter is only related to the vehicle mass and the ground material and remains constant under the same operating conditions.

[0081] Interference resistance It is positively correlated with the wind turbine load torque variable, and the specific conversion relationship is as follows:

[0082]

[0083] in, The current moment represents the wind turbine load torque variable. is the transmission coefficient of the wind turbine load torque to the drive wheel shaft system, and is a fixed parameter obtained in advance through dynamic calibration.

[0084] (1) Calibration method: Offline calibration is performed using the static load calibration method (completed before the robot leaves the factory, requiring calibration only once during its entire lifecycle). The steps are as follows:

[0085] The robot's drive wheels are completely fixed with a special clamp, keeping the chassis absolutely stationary and eliminating the influence of ground friction.

[0086] A high-precision torque sensor (measurement accuracy ±0.1 N·m) is installed between the drive wheel shaft and the fixture to directly measure the static disturbance torque transmitted from the wind turbine load to the drive wheel shaft. ;

[0087] Control the cleaning fan to run at the rated speed. By adjusting the opening of the electric throttle valve in the fan's air inlet duct, the fan load torque is gradually increased from 0 to the rated load torque, and each load point is stably operated for 5 seconds.

[0088] Two sets of data are collected simultaneously at each load point:

[0089] The output torque of the fan is calculated from the q-axis current component of the fan drive motor. ; Let be the torque constant of the fan drive motor. This represents the q-axis current component of the fan drive motor.

[0090] Disturbance torque of drive wheel axle measured by torque sensor ;

[0091] According to the formula Calculate the transfer coefficient for each load point, and take the average of the calculation results for all load points as the final calibration value.

[0092] (2) Applicable conditions and error correction: This calibration method is applicable to normal operating conditions with rigid chassis structures and no relative slippage between the drive wheels and the ground. For scenarios with flexible chassis or soft ground (ground deformation > 5 mm), a correction factor is introduced. (Value range 0.9 to 1.1) for Corrections were made, and the correction coefficients were determined through on-site no-load coasting tests. During calibration, the effect of ambient temperature changes on the stiffness of the transmission system was negligible.

[0093] By solving the above chassis dynamics equations, the theoretical acceleration of the vehicle body is obtained. The theoretical acceleration is then integrated to obtain the theoretical driving wheel linear velocity at the current moment. The initial value of the integral is the theoretical driving wheel linear velocity at the previous moment.

[0094] Calculate the chassis slip ratio at the current moment. The formula for calculating the slip ratio is:

[0095]

[0096] in, The chassis slip ratio at the current moment; This represents the theoretical linear velocity of the driving wheel at the current moment.

[0097] When the chassis slip ratio is greater than the preset slip ratio threshold, the negative pressure change rate of the real-time negative pressure value at the current moment is extracted as the dust concentration feature, and a dust concentration gradient field with the current coordinate point is constructed by combining the robot's historical pose data.

[0098] (1) Theoretical basis and experimental verification of the mapping relationship: The rate of change of negative pressure in the fan inlet duct is linearly positively correlated with the dust concentration. The physical essence is that the greater the mass of dust inhaled per unit time, the faster the flow cross-sectional area of ​​the inlet duct decreases, resulting in a faster rate of increase in negative pressure. This invention has been universally verified using a standard dust generating device: using A1 fine dust, A2 coarse dust, and A3 ultrafine dust as specified in ISO12103-1, calibration curves of negative pressure change rate and dust concentration were established under the conditions of humidity from 30% to 80% and fan speed from 50% to 100% of rated speed. The experimental results show that the linear correlation coefficient R2 under different operating conditions is ≥0.92, verifying the universality of this mapping relationship.

[0099] (2) Multi-factor correction method: To address the influence of different dust types, humidity conditions and fan characteristics, a three-dimensional correction coefficient matrix is ​​introduced. Correction was applied to the negative pressure change rate, resulting in the corrected dust concentration characteristics. for:

[0100]

[0101] in, Dust type (fine / coarse / ultrafine); The relative humidity of the environment; This is the current speed of the fan. This represents the original negative pressure change rate. The correction coefficient matrix is ​​established through pre-calibration experiments and stored in the robot's local storage unit, automatically retrieved during runtime based on real-time environmental parameters. For unknown types of dust, an adaptive correction algorithm is employed: when the robot enters a new area, it first estimates the coefficients using default values, and then iteratively updates the correction coefficients online based on the amount of residual dust on the ground after cleaning (detected by a visual sensor).

[0102] The preset slip ratio threshold is a fixed value set in advance, ranging from 0.1 to 0.3. When the chassis slip ratio exceeds the preset slip ratio threshold, the robot is determined to be in a high-risk slip condition, and the dust concentration gradient field construction process is initiated. When extracting the negative pressure change rate, a fixed time window is used to extract the time series of real-time negative pressure values. The time window ranges from 0.5s to 2s, and the extracted negative pressure time series is the set of sampling points within the corresponding time window. The first-order difference is performed on the extracted negative pressure time series to obtain the negative pressure change rate. The calculation formula is as follows:

[0103]

[0104] in, This represents the rate of change of negative pressure at the corresponding sampling time. This represents the real-time negative pressure value at the corresponding sampling time. The sampling period is the reciprocal of the sampling frequency.

[0105] The negative pressure change rate at the current moment is used as a feature of dust concentration. Dust concentration and negative pressure change rate are positively correlated. When the robot passes through a high dust concentration area, the amount of dust sucked in by the fan increases, the air intake resistance rises rapidly, and the rate of change of negative pressure value with time increases. Therefore, the negative pressure change rate can directly characterize the dust concentration level at the corresponding location.

[0106] The coordinate sequence in the historical pose data corresponding to the fixed time window is retrieved. The historical pose data is output in real time by the pose measurement unit and stored in the local storage unit. The stored pose data includes the robot's global coordinates and heading angle corresponding to each sampling moment. The coordinate sequence and the negative pressure time sequence have corresponding timestamps to ensure that each negative pressure sampling point can be matched with the corresponding spatial coordinate point.

[0107] Each coordinate point in the coordinate sequence is taken as a spatial node, and the negative pressure change rate corresponding to each spatial node is mapped to the dust concentration estimate of that spatial node. The mapping relationship is a linear positive correlation mapping, that is, the dust concentration estimate is equal to the product of the concentration conversion coefficient and the corresponding negative pressure change rate, where the concentration conversion coefficient is a pre-calibrated fixed parameter.

[0108] Centered on the current coordinate point, a preset spatial interpolation algorithm is used to interpolate the estimated dust concentration values ​​of adjacent spatial nodes, generating a dust concentration gradient field containing spatial distribution information. The preset spatial interpolation algorithm employs an inverse distance weighted interpolation algorithm. The neighborhood for interpolation is a square region centered on the current coordinate point, with a pre-defined fixed side length ranging from 5m. 20m. The formula for calculating inverse distance weighted interpolation is:

[0109]

[0110] in, represents the estimated dust concentration corresponding to the raster coordinates to be interpolated; M is the number of spatial nodes within the neighborhood. This is the estimated dust concentration value corresponding to the i-th spatial node in the neighborhood; p is the Euclidean distance between the raster coordinates to be interpolated and the i-th spatial node; p is the distance weight exponent, with a value ranging from 1. 3.

[0111] By interpolation, the dust concentration estimate for each grid cell within the neighborhood is obtained. The gradient of the dust concentration estimate for each grid cell is then calculated to obtain the dust concentration gradient field. The gradient calculation uses the central difference method, and the formula is as follows:

[0112]

[0113] in, This represents the dust concentration gradient field vector at the corresponding grid coordinates. , These are the raster resolutions of the raster map in the x and y directions, respectively, and are preset fixed values.

[0114] In this embodiment, the core interpolation calculation parameters involved in the construction of the dust concentration gradient field are shown in the table below. By fixing the values ​​of the above parameters, the consistency of the interpolation calculation process and the spatial resolution of the dust concentration gradient field meet the requirements of path planning are ensured.

[0115] Table 1. Parameters for interpolation calculation of dust concentration gradient field

[0116]

[0117] The parameters defined in this table are all pre-calibrated fixed parameters that remain constant during robot operation, ensuring stable input conditions for the construction of the dust concentration gradient field and avoiding distortion of the gradient field spatial distribution due to parameter fluctuations.

[0118] The chassis slip ratio and the dust concentration gradient field are fused to generate a comprehensive cost map. The preset basic occupancy grid map is a global grid map pre-constructed by the robot using simultaneous localization and mapping (SLAM) algorithms. Each grid in the basic occupancy grid map is marked as an empty grid or an obstacle grid, where an empty grid represents a traversable area for the robot, and an obstacle grid represents a traversable area. In the basic occupancy grid map, the dust concentration estimate corresponding to each grid in the dust concentration gradient field is obtained. The grid coordinates and the dust concentration gradient field coordinates use the same global coordinate system to ensure consistent coordinate mapping. The dust concentration estimate is input into a preset slip penalty function to calculate the slip penalty cost for each grid. The slope of the slip penalty function is positively correlated with the chassis slip ratio. The expression of the slip penalty function is:

[0119]

[0120] in, The value of the slip penalty for the corresponding grid; is the penalty coefficient, which is a pre-set fixed parameter.

[0121] Obtain the base passage cost value of free grid cells in the base occupied grid map. The base passage cost value is a pre-set fixed value. The base passage cost value of all free grid cells is a fixed constant, while the base passage cost value of obstacle grid cells is infinite. The base passage cost value of each free grid cell is weighted and summed with its corresponding slip penalty cost value. The sum is used as the comprehensive cost value of that free grid cell in the comprehensive cost map. The calculation formula is as follows:

[0122]

[0123] in, The comprehensive cost of the corresponding grid; The basic pass value for the corresponding grid; , The weighting coefficients for the base passage cost and the slippage penalty cost are respectively, satisfying... This is a pre-defined fixed value. For obstacle grids, their comprehensive cost is set to infinity to ensure that obstacle grids are not selected as path nodes during path planning.

[0124] The robot's trajectory is generated using a path planning algorithm, constrained by reducing the slip ratio cost in the comprehensive cost map and moving along the gradient descent direction of the dust concentration gradient field. The path planning algorithm employs a heuristic path search algorithm based on a grid map, using the robot's current coordinates as the starting node and expanding nodes within the comprehensive cost map.

[0125] This invention specifically employs an improved A* algorithm as the core path planning algorithm, which, based on the traditional A* algorithm, uses the evaluation cost as the actual cost of a node. And retain the heuristic function

[0126] Calculate the total cost (using Manhattan distance). This improves path search efficiency.

[0127] To address the real-time replanning problem in dynamic environments, a hierarchical replanning mechanism is adopted:

[0128] Global replanning: When the updated range of the comprehensive cost map exceeds 5m×5m, or when more than 30% of the grid cost on the current path increases beyond the threshold, global replanning is triggered to regenerate the complete path from the current location to the target area. The replanning cycle is ≤200ms.

[0129] Local dynamic obstacle avoidance: The Dynamic Window Method (DWA) is used as the local planner. The global path is used as the reference trajectory to detect dynamic obstacles (such as pedestrians and mobile devices) in real time. The speed and direction of travel are adjusted in the local area to avoid dynamic obstacles and then re-track the global path. The local planning cycle is ≤50ms.

[0130] Emergency braking mechanism: When a sudden obstacle is detected within 1m ahead, the emergency braking is immediately triggered, and the fan stops running to avoid collision and dust spread.

[0131] The set of adjacent extended nodes of the starting node is determined. The adjacent extended nodes are the free grid cells in the 8-neighborhood of the starting node whose comprehensive cost is not infinite, ensuring that all extended nodes are within a passable area. The direction vector of the line connecting the starting node to each node in the set of adjacent extended nodes is calculated. For a corresponding extended node, the direction vector is a two-dimensional vector formed by the difference between the coordinates of the extended node and the coordinates of the starting node. The negative gradient direction vector of the starting node in the dust concentration gradient field is calculated. The negative gradient direction vector is the opposite direction of the gradient vector of the gradient field at the starting node. The negative gradient direction is the gradient descent direction of the dust concentration gradient field. Moving along this direction allows the robot to travel to areas with lower dust concentration, reducing the risk of slippage caused by the fan load.

[0132] Calculate the cosine of the angle between each of the connecting direction vectors and the negative gradient direction vector, using the following formula:

[0133]

[0134] in, Let cosine be the included angle value corresponding to the i-th extended node; The direction vector of the corresponding connecting line; This corresponds to the negative gradient direction vector; It is the dot product of two vectors; , These are the magnitudes of the two vectors, respectively.

[0135] The range of the cosine of the angle is [-1, 1]. When the direction of the connecting line is consistent with the direction of the negative gradient, the cosine of the angle takes the maximum value of 1, and the path direction and the gradient descent direction have the highest degree of fit. When the direction of the connecting line is opposite to the direction of the negative gradient, the cosine of the angle takes the minimum value of -1, and the degree of fit is the lowest.

[0136] The evaluation value is obtained by adding the cosine value of the included angle to the comprehensive cost value of the nodes in the adjacent extended node set. The calculation formula is as follows:

[0137]

[0138] in, Let $i$ be the evaluation value corresponding to the $i$-th extended node. The total cost of the corresponding extended nodes; The gradient direction weight coefficient is a pre-defined fixed parameter.

[0139] In this invention, all weight coefficients (including the basic passage cost weight w1, the slip penalty cost weight w2, and the gradient direction weight coefficient) Distance penalty coefficient All (etc.) were determined using orthogonal experimental calibration combined with adaptive optimization strategies:

[0140] (1) Initial calibration method: A four-factor, three-level orthogonal experiment was designed, using path tracking error, sweeping coverage, and average slip rate as evaluation indicators, and the experiment was conducted under typical high-dust operation scenarios. The optimal initial values ​​of each weight coefficient were determined through range analysis, and the initial value ranges are as follows:

[0141] : 0.2~0.6; 0.4~0.8

[0142] : 0.5~2.0; 0.1~0.5

[0143] (2) Adaptive optimization strategy: During robot operation, the weight coefficients are dynamically adjusted according to the real-time working conditions.

[0144] When the chassis slip ratio is greater than 0.5, increase w2 by 20%. Increase by 30%, prioritizing the avoidance of slippage risks;

[0145] When the path tracking error exceeds 0.2m, Reduce by 20% to decrease distance penalty and improve path smoothness;

[0146] When the cleaning coverage is less than 90%, Reduce the weighting by 20% and appropriately increase the cleaning probability of high-dust areas. The weighting coefficients are adjusted incrementally, with each adjustment not exceeding 50% to avoid system oscillations.

[0147] The node with the lowest evaluation value is selected as the target child node, and the expansion process is repeated until the termination condition is met. The termination condition is that the distance between the new starting node and the boundary of the preset target cleaning area is less than a preset distance threshold. The path connecting all target child nodes is smoothed to obtain the final travel trajectory. The smoothing process uses B-spline curve fitting to ensure the curvature of the travel trajectory is continuous and meets the robot's chassis kinematic constraints.

[0148] Based on the curvature of the travel trajectory, the torque of the robot's drive wheels and the power of the cleaning fan are adjusted. The travel trajectory is discretized to obtain a sequence of trajectory points composed of multiple discrete trajectory points. The discretization step size is consistent with the resolution of the grid map, and each discrete trajectory point corresponds to a grid coordinate in the comprehensive cost map. The trajectory curvature corresponding to each discrete trajectory point is calculated. The curvature calculation uses the three-point method. For the j-th point in the discrete trajectory point sequence, the two adjacent points are taken, and the curvature is calculated using the radius of the arc determined by the three points. The calculation formula is:

[0149]

[0150] in, Let be the trajectory curvature of the j-th discrete trajectory point; This represents the Euclidean distance between the two corresponding points.

[0151] When the trajectory curvature of the discrete trajectory points is greater than a preset curvature threshold, the robot's current steering angular velocity is obtained. A torque adjustment is calculated based on the difference between the current steering angular velocity and the trajectory curvature, and this torque adjustment is then added to the reference value of the drive wheel torque. The preset curvature threshold is a pre-set fixed value with a range of 0.1m. -1 0.5m -1 When the trajectory curvature of a discrete trajectory point exceeds a preset curvature threshold, the robot is determined to be in a turning state, and the adjustment process for drive wheel torque and fan power is initiated. The current steering angular velocity is acquired in real time by the inertial measurement unit, and the formula for calculating the torque adjustment is:

[0152]

[0153] in, This corresponds to the torque adjustment amount; This is the torque adjustment gain coefficient, which is a pre-calibrated fixed parameter; This represents the robot's turning angular velocity at the current moment; This represents the theoretical turning angular velocity corresponding to the trajectory curvature.

[0154] The torque adjustment is superimposed on the reference value of the drive wheel torque to obtain the adjusted drive wheel torque. The reference value of the drive wheel torque is the preset torque value when the robot is moving in a straight line. For the drive wheels on the left and right sides, positive and negative torque adjustment amounts are superimposed according to the steering direction to realize differential steering control. This ensures that the actual steering angular velocity of the robot matches the theoretical steering angular velocity corresponding to the trajectory curvature, reducing the risk of drive wheel slippage under turning conditions.

[0155] The estimated centrifugal acceleration on the side where the cleaning fan is located is calculated based on the trajectory curvature. The product of the fan power and the estimated centrifugal acceleration is used as a power suppression parameter. The initial power of the cleaning fan is subtracted from the power suppression parameter to obtain the final output power. The initial power is the rated power of the fan when the robot is moving in a straight line. The estimated centrifugal acceleration is positively correlated with the trajectory curvature. The greater the trajectory curvature, the greater the centrifugal acceleration generated during turning, and the more obvious the disturbance of the fan load on the chassis attitude. Therefore, the fan power is reduced by using the power suppression parameter to reduce the disturbance effect of the fan load. The calculation formula for power adjustment is:

[0156]

[0157] in, This refers to the final output power of the fan; This represents the initial power of the fan; The power suppression coefficient is a pre-set fixed parameter. Estimation of centrifugal acceleration under turning conditions.

[0158] Set a lower limit for the fan power to ensure that the final output power is not lower than the minimum power threshold required to maintain the negative pressure of the fan, so as to avoid the cleaning and dust collection function from failing due to excessively low power. The minimum power threshold is a fixed percentage of the rated power of the fan.

[0159] This embodiment fully realizes the entire process of robot cleaning control and path planning in harsh environments. By converting the real-time negative pressure value of the cleaning fan into a fan load torque variable and introducing it into the chassis slip ratio calculation, real-time perception of slip risk caused by fan load is achieved. By constructing a dust concentration gradient field and fusing it to generate a comprehensive cost map, the path planning process simultaneously considers slip risk and dust distribution characteristics. Finally, by adjusting the torque and power corresponding to the trajectory curvature, the coordination of cleaning operation and chassis motion control is achieved, ensuring the robot's path tracking accuracy and cleaning operation continuity in high dust environments.

[0160] In a preferred embodiment, this embodiment refines and optimizes the calculation process of the fan load torque variable. By combining the aerodynamic characteristics of the fan with the measured parameters of the motor, the calculation accuracy of the fan load torque variable is improved, thereby improving the accuracy of the chassis slip ratio calculation.

[0161] refer to Figure 2 The current rotational speed of the cleaning fan is obtained. This speed is acquired in real-time by the encoder of the fan drive motor, and the sampling frequency is synchronized with the sampling frequencies of the drive wheel speed signal and the negative pressure signal. The ratio of the real-time negative pressure value to the current rotational speed is used as the aerodynamic drag coefficient, calculated using the following formula:

[0162] in, This represents the aerodynamic drag coefficient at the current moment; The current speed of the cleaning fan.

[0163] The aerodynamic drag coefficient is multiplied by the square of the current rotational speed, and combined with a preset mechanical friction compensation coefficient, to calculate the fan load torque variable. The calculation formula is as follows:

[0164]

[0165] in, It is a unit conversion factor used to convert the product of speed and aerodynamic drag coefficient into torque, and is a pre-calibrated fixed parameter; It is the mechanical friction compensation coefficient, used to compensate for the mechanical friction torque of the fan bearings and transmission mechanism.

[0166] The drive wheel speed signal is converted into drive wheel linear velocity using the same conversion formula as in the previous embodiment. The fan load torque variable is introduced as an interference term into the robot's chassis dynamics equations. These equations consider the impact of the fan load torque on the vehicle's longitudinal dynamics. Changes in the fan load torque cause fluctuations in the output power of the fan drive motor, which in turn causes fluctuations in the bus voltage of the vehicle's power system, affecting the output torque of the drive wheel motor. Simultaneously, changes in the aerodynamic load of the fan alter the vehicle's center of gravity distribution, leading to changes in the normal load on the drive wheels and thus affecting their adhesion limits. In this embodiment, the chassis dynamics equations use a vehicle longitudinal dynamics model, expressed as:

[0167]

[0168] in, The moment of inertia of the drive wheel is a pre-calibrated fixed parameter. This refers to the angular acceleration of the drive wheel; Let be the angular velocity of the drive wheel, and its conversion to the speed of the drive wheel is as follows: .

[0169] By solving the above chassis dynamics equations, the theoretical angular velocity of the drive wheel is obtained, which is then converted into the theoretical linear velocity of the drive wheel. The difference between the theoretical linear velocity of the drive wheel and the linear velocity of the drive wheel is divided by the theoretical linear velocity of the drive wheel to obtain the chassis slip ratio at the current moment. The calculation formula is consistent with the aforementioned embodiment.

[0170] The three-phase current signal of the blower drive motor driving the cleaning blower is acquired in real time. The three-phase current signal is collected in real time by the current sampling circuit of the blower drive motor, and the sampling frequency is synchronized with the sampling frequency of the blower speed. The three-phase current signal is subjected to Clarke transform and Park transform to extract the d-axis current component and q-axis current component.

[0171] The formula for calculating the Clark transform is:

[0172] The formula for calculating the Park transform is:

[0173]

[0174] in, , , This is the three-phase current signal for the fan drive motor; , The current component is in the stationary coordinate system; , These are the d-axis and q-axis current components in the rotating coordinate system; The rotor electrical angle of the fan drive motor is obtained in real time by the motor encoder.

[0175] Multiplying the q-axis current component by the torque constant of the cleaning fan yields the measured output torque, calculated using the following formula:

[0176]

[0177] in, This represents the measured output torque of the fan drive motor at the current moment. is the torque constant of the fan drive motor, and is a fixed parameter specified on the motor nameplate.

[0178] The theoretical aerodynamic load torque is obtained by multiplying the aerodynamic drag coefficient by the square of the current rotational speed. The calculation formula is as follows:

[0179] The difference between the measured output torque and the theoretical aerodynamic load torque is taken as the observed value of the dynamic friction torque, i.e.:

[0180] The dynamic friction torque observations include non-aerodynamic load torques such as the mechanical friction torque and wind resistance loss torque of the wind turbine. These dynamic friction torque observations are filtered using a first-order Butterworth low-pass filter with a pre-set fixed cutoff frequency. The calculation formula for the filtering is as follows:

[0181]

[0182] in, These are the filter coefficients, with values ​​ranging from 0.8. 0.95 is a preset fixed value; This is the updated mechanical friction compensation coefficient at the current moment.

[0183] The filtered result is substituted into the calculation formula of the wind turbine load torque variable as the updated mechanical friction compensation coefficient, thereby realizing the real-time dynamic update of the mechanical friction compensation coefficient and eliminating the influence of friction torque changes caused by factors such as temperature changes and mechanical wear on the accuracy of load torque calculation during wind turbine operation.

[0184] In this embodiment, the core parameters involved in the calculation of wind turbine load torque are shown in the table below. By fixing the values ​​of the above parameters, the calculation accuracy of the wind turbine load torque variable is ensured to meet the requirements of slip ratio calculation.

[0185] Table 2 Core Parameters for Calculating Fan Load Torque

[0186]

[0187] Among the parameters defined in this table, the fan torque constant, unit conversion factor, and drive wheel moment of inertia are fixed calibration parameters that remain constant throughout the robot's entire life cycle; the low-pass filter coefficient, filter cutoff frequency, and torque transmission coefficient are configurable parameters that can be adjusted according to the robot's actual operating conditions.

[0188] This embodiment constructs a theoretical calculation model for the wind turbine load torque by calculating the aerodynamic drag coefficient of the wind turbine. At the same time, it extracts the measured output torque by using the three-phase current signal of the wind turbine drive motor, realizing the dynamic update of the mechanical friction compensation coefficient. This effectively improves the calculation accuracy of the wind turbine load torque variable, thereby improving the accuracy of the chassis slip ratio calculation. This enables the robot to more accurately perceive the slip risk caused by the wind turbine load, providing reliable input data for subsequent path planning and control adjustment.

[0189] In another preferred embodiment, this embodiment refines and optimizes the construction process of the dust concentration gradient field. By introducing the rotation speed signal of the cleaning brush, the dust concentration estimate is corrected, eliminating the influence of non-dust disturbance factors on the negative pressure change rate, and improving the spatial distribution accuracy of the dust concentration gradient field.

[0190] refer to Figure 3 The real-time negative pressure value is captured within a fixed time window. The negative pressure change rate is calculated by first-order difference of the captured negative pressure time series, and the calculation process is consistent with the previous embodiment. The coordinate sequence from the historical pose data corresponding to the fixed time window is retrieved. The coordinate sequence and the negative pressure time series both have corresponding timestamps. The brush rotation speed signal of the robot's cleaning brush within the fixed time window is synchronously acquired. The brush rotation speed signal is collected in real time by the encoder of the brush drive motor, and the sampling frequency is synchronized with the sampling frequency of the negative pressure signal.

[0191] Extract continuous high-speed periods from the brush rotation speed signal that exceed a preset speed threshold. The preset speed threshold is a fixed percentage of the brush's rated speed, a pre-set fixed value. Mark the spatial nodes in the coordinate sequence corresponding to these continuous high-speed periods as effective disturbance nodes. Effective disturbance nodes indicate that the brush is in normal cleaning operation at that position, and the brush rotation raises dust from the ground, allowing the fan to suck in the raised dust. In this case, the negative pressure change rate accurately reflects the dust concentration at that location. For spatial nodes corresponding to non-continuous high-speed periods, mark them as ineffective disturbance nodes. Ineffective disturbance nodes indicate that the brush is slowing down or stopped at that position, failing to effectively raise dust from the ground. In this case, the negative pressure change rate does not accurately reflect the dust concentration at that location and requires correction.

[0192] For the effective disturbance node, the ratio of the negative pressure change rate to the brush rotation speed signal at the corresponding moment is calculated as the standardized concentration feature, and the calculation formula is:

[0193]

[0194] in, This represents the standardized concentration characteristics at the corresponding time point; This is the brush rotation speed signal at the corresponding moment.

[0195] The standardized concentration characteristics eliminate the influence of brush rotation speed variation on negative pressure change rate, making the dust concentration estimate under different brush rotation speeds have a unified measurement standard.

[0196] For ineffective perturbation nodes, the standardized concentration feature is reduced by a time decay factor, with the time decay factor ranging from 0.8. 0.99 is a pre-set fixed value. The formula for the decrementing process is as follows:

[0197]

[0198] Where λ is the time decay factor; This is the time corresponding to the previous valid perturbation node; This represents the standardized concentration feature corresponding to the previous effective perturbation node.

[0199] By reducing the time decay factor, the dust concentration estimate of non-effective disturbance nodes gradually decreases over time, avoiding misjudgment of dust concentration distribution due to the distortion of negative pressure change rate caused by the brush stoppage.

[0200] The standardized concentration features of the effective perturbation nodes and the features of the ineffective perturbation nodes after reduction are respectively used as the dust concentration estimates of the corresponding spatial nodes. That is, the dust concentration estimates are equal to the product of the corrected concentration conversion coefficient and the corresponding standardized concentration features, wherein the corrected concentration conversion coefficient is a pre-calibrated fixed parameter.

[0201] Centered on the current coordinate point, a preset spatial interpolation algorithm is used to interpolate the estimated dust concentration values ​​of adjacent spatial nodes, generating a dust concentration gradient field containing spatial distribution information. In this embodiment, the spatial interpolation algorithm is the Kriging interpolation algorithm. Compared with the inverse distance weighted interpolation algorithm, the Kriging interpolation algorithm can consider the spatial autocorrelation between spatial nodes, improving the accuracy of the interpolation results.

[0202] To address the high computational complexity of the Kriging interpolation algorithm, this invention employs a three-level optimization strategy to ensure real-time performance at a sampling frequency of 200Hz:

[0203] Interpolation region clipping: Kriging interpolation is performed only on a local region of 3m×3m around the robot's current position. Regions outside this range are quickly estimated using an inverse distance-weighted interpolation algorithm. The 3m×3m local region contains approximately 900 grid cells, far less computationally intensive than global interpolation.

[0204] Pre-calculation of variogram: The variogram parameters for different dust scenarios are pre-calibrated and stored as a lookup table. At runtime, the corresponding parameters are directly called based on the current dust concentration level, avoiding real-time calculation of the variogram.

[0205] Hardware-accelerated computation: The matrix operations of Kriging interpolation are accelerated using the MCU's FPU (Floating-Point Unit), while the NEON instruction set is used to optimize the solution process of the linear equation system. Actual testing shows that on the STM32H750 chip, the Kriging interpolation calculation for a 3m×3m area takes ≤2ms, fully meeting the real-time requirements at a 200Hz sampling frequency.

[0206] In addition, this invention retains inverse distance weighted interpolation as an alternative algorithm. When the system load exceeds 80%, it automatically switches to inverse distance weighted interpolation to ensure system stability.

[0207] The formula for calculating Kriging interpolation is:

[0208]

[0209] in, The Kriging weights are obtained by solving the system of equations for the variogram.

[0210] The variogram uses a spherical model and is expressed as follows:

[0211]

[0212] Where h is the distance between the two spatial nodes; C is the nugget value; C is the base value; a is the range, all of which are pre-calibrated parameters.

[0213] The dust concentration estimate for each grid cell within the neighborhood is obtained by Kriging interpolation, and the dust concentration gradient field is then calculated by the central difference method. The gradient calculation process is consistent with the aforementioned embodiment.

[0214] In this embodiment, the core parameters involved in the dust concentration estimation correction process are shown in the table below. By fixing the values ​​of the above parameters, it is ensured that the dust concentration estimation can truly reflect the actual dust distribution in the work area.

[0215] Table 3 Correction Parameters for Estimated Dust Concentration Values

[0216]

[0217] Among the parameters defined in this table, the rated speed of the brush disc is the fixed nameplate parameter of the brush disc drive motor, and the other parameters are all configurable parameters that can be adjusted according to factors such as dust characteristics and ground material in the cleaning operation scenario.

[0218] This embodiment introduces the rotation speed signal of the cleaning brush to standardize and correct the dust concentration characteristics obtained from the negative pressure change rate and perform time decay processing. This effectively eliminates the interference of brush operation status changes on the dust concentration estimate. At the same time, the Kriging interpolation algorithm is used to improve the interpolation accuracy of the spatial distribution of dust concentration, so that the constructed dust concentration gradient field can more realistically reflect the dust distribution characteristics of the working area, providing accurate spatial distribution data for subsequent integrated cost map fusion and path planning.

[0219] In another preferred embodiment, this embodiment refines and optimizes the fusion process of the comprehensive cost map, the node expansion constraints of path planning, and the adjustment process of drive wheel torque and wind turbine power under turning conditions, thereby further improving the rationality of path planning and the control stability under turning conditions.

[0220] refer to Figure 4 The chassis slip ratio is fused with the dust concentration gradient field to generate a comprehensive cost map. In a preset basic occupancy grid map, the estimated dust concentration value corresponding to each grid cell in the dust concentration gradient field is obtained. The basic occupancy grid map is a probabilistic occupancy grid map, with the occupancy probability of each grid cell ranging from 0 to 1. 1. When the occupancy probability is greater than 0.5, it is marked as an obstacle grid; when the occupancy probability is less than 0.5, it is marked as an idle grid; when the occupancy probability is equal to 0.5, it is marked as an unknown grid. The comprehensive cost of the unknown grid is set to a fixed value that is greater than that of the idle grid and less than that of the obstacle grid.

[0221] The estimated dust concentration is input into a preset slip penalty function to calculate the slip penalty cost for each grid cell. The slope of the slip penalty function is positively correlated with the chassis slip ratio. The slip penalty function is a piecewise linear function. Specifically, when the chassis slip ratio is less than or equal to a preset slip ratio threshold, the slope of the slip penalty function is 0, i.e., the slip penalty cost is 0. When the chassis slip ratio is greater than the preset slip ratio threshold, the slope of the slip penalty function increases linearly with the increase of the chassis slip ratio. The expression of the slip penalty function is:

[0222]

[0223] in, This is the preset slip ratio threshold.

[0224] By using a piecewise linear slip penalty function, the dust concentration distribution is only incorporated into the cost map calculation when the robot detects a high slip risk, thus avoiding unnecessary interference from the dust concentration distribution to path planning under low slip risk conditions.

[0225] The basic access value of the free grid cells in the basic occupied grid map is obtained. In this embodiment, the basic access value of the free grid cells is positively correlated with the occupation probability of the grid cells. The higher the occupation probability, the greater the basic access value. The calculation formula is as follows:

[0226]

[0227] in, This represents the occupancy probability of the corresponding grid cell, with a value ranging from 0. 0.5.

[0228] The basic passage cost value and the corresponding slip penalty cost value of each free grid are weighted and summed, and the sum is used as the comprehensive cost value of the free grid in the comprehensive cost map. The weighting coefficients are dynamically adjusted and are positively correlated with the chassis slip rate at the current time. Specifically, the formula for calculating the weight of the slip penalty cost value is as follows:

[0229] Weight of basic passage cost value By dynamically adjusting the weighting coefficients, the higher the chassis slip rate, the greater the proportion of slip penalty value in the overall value, and the higher the priority of slip risk avoidance during path planning. The overall value of obstacle grids is set to infinity, and the overall value of unknown grids is set to a pre-defined fixed value.

[0230] refer to Figure 5 The robot's trajectory is generated using a path planning algorithm, constrained by reducing the slip ratio cost in the comprehensive cost map and moving along the gradient descent direction of the dust concentration gradient field. Using the robot's current coordinates as the starting node, a set of adjacent extended nodes is determined in the comprehensive cost map. In this embodiment, the set of adjacent extended nodes is the 8-neighbor grid of the starting node, while excluding obstacle grids with infinite comprehensive cost values.

[0231] Obtain the robot's chassis wheelbase and maximum steering angle, and calculate the robot's minimum turning radius using the following formula:

[0232]

[0233] in, B is the robot's minimum turning radius; B is the robot's chassis wheelbase, which is the lateral distance between the left and right drive wheels, and is a fixed parameter. This is the maximum permissible steering angle of the robot's steering mechanism, and it is a fixed parameter.

[0234] Determine whether the distance from the starting node to a node in the set of adjacent extended nodes is less than the minimum turning radius. If it is less than the minimum turning radius, it means the robot cannot reach the extended node by traveling in a straight line and needs to adjust its direction. In this case, project the connecting direction vector onto the tangent direction of the arc centered at the starting node and with the minimum turning radius as the radius to obtain the corrected direction vector. The projection calculation formula is as follows:

[0235]

[0236] in, To correct the direction vector; It is the radial vector pointing from the starting node to the extended node, and it is the same as the direction vector of the connection.

[0237] Calculate the cosine of the angle between the corrected direction vector and the negative gradient direction vector, and add the reciprocal of the distance as a distance penalty term to the calculation of the evaluation cost value, replacing the original comprehensive cost value in the summation calculation. The formula for calculating the evaluation cost value is:

[0238]

[0239] in, This is the distance penalty coefficient, a pre-set fixed parameter; This is the distance from the starting node to the extended node; This is to correct the cosine value of the angle between the direction vector and the negative gradient direction vector.

[0240] Through the above correction process, it is ensured that the path nodes generated during path planning meet the robot's minimum turning radius constraint, avoiding the generation of unexecutable path trajectories. At the same time, the distance penalty term reduces the probability of selecting nodes that are too close during path planning, thereby improving the smoothness of the path.

[0241] The node with the lowest evaluation value is selected as the target child node, and the expansion process is repeated until the termination condition is met. The termination condition is that the new starting node reaches the preset target cleaning area. The path connecting all target child nodes is smoothed using cubic spline curves to obtain a continuous curvature travel trajectory.

[0242] refer to Figure 6Based on the curvature of the travel trajectory, the torque of the robot's drive wheels and the power of the cleaning fan are adjusted. The travel trajectory is discretized to obtain a sequence of trajectory points composed of multiple discrete trajectory points, and the trajectory curvature corresponding to each discrete trajectory point is calculated, the calculation process being consistent with the previous embodiment. When the trajectory curvature of the discrete trajectory point is greater than a preset curvature threshold, the current turning angular velocity of the robot is obtained, and the torque adjustment amount is calculated based on the difference between the current turning angular velocity and the trajectory curvature. The torque adjustment amount is then superimposed on the reference value of the drive wheel torque. In this embodiment, the torque adjustment adopts a PID control algorithm, and the calculation formula for the torque adjustment amount is:

[0243]

[0244] in, The tracking error is the steering angular velocity. , , These are the proportional coefficient, integral coefficient, and derivative coefficient of the PID controller, respectively, and are pre-calibrated parameters.

[0245] The torque adjustment is superimposed on the reference value of the drive wheel torque. For the left and right drive wheels, opposite positive and negative torque adjustments are applied respectively according to the steering direction to realize closed-loop control of differential steering and improve the trajectory tracking accuracy under turning conditions.

[0246] The estimated centrifugal acceleration on the side where the cleaning fan is located is calculated based on the trajectory curvature. The product of the fan power and the estimated centrifugal acceleration is used as a power suppression parameter. The installation offset distance and installation offset angle of the robot's cleaning fan in the vehicle coordinate system are obtained. The origin of the vehicle coordinate system is the geometric center of the robot chassis, the x-axis points directly forward of the robot, and the y-axis points to the left side of the robot. The installation offset distance is the straight-line distance from the fan installation position to the coordinate origin, and the installation offset angle is the angle between the line connecting the fan installation position and the coordinate origin and the x-axis, which are pre-calibrated fixed parameters.

[0247] Based on the trajectory curvature of the discrete trajectory points and the robot's current travel velocity, the robot's current lateral acceleration is calculated. The formula for calculating the lateral acceleration is:

[0248]

[0249] The direction of the lateral acceleration is the same as the robot's turning direction, pointing towards the center of the turning trajectory.

[0250] The current lateral acceleration is decomposed into the direction corresponding to the installation offset angle, and the estimated centrifugal acceleration of the cleaning fan is calculated by combining the installation offset distance. The calculation formula is as follows:

[0251]

[0252] in, Estimate the centrifugal acceleration of the cleaning fan; This refers to the installation offset distance of the fan; This refers to the installation offset angle of the fan; Let be the radius of curvature of the turning trajectory.

[0253] The product of the fan power and the estimated centrifugal acceleration is used as the power suppression parameter. The formula for calculating the power suppression parameter is as follows:

[0254]

[0255] in, is the power suppression parameter; g is the gravitational acceleration.

[0256] The initial power of the cleaning blower is subtracted from the power suppression parameter to obtain the final output power. Simultaneously, the q-axis current component obtained through the three-phase current transformation of the blower drive motor in the previous embodiment is retrieved. When the q-axis current component exceeds a preset overcurrent threshold, the power suppression parameter is multiplied by a forced attenuation coefficient greater than 1, and then subtracted from the initial power. The forced attenuation coefficient has a value range of 1.2. 2.0 is a preset fixed value, while limiting the lower limit of the fan power to not be lower than the minimum power threshold for maintaining negative pressure, so as to avoid the cleaning function from failing due to excessively low fan power, and to prevent the fan drive motor from being damaged due to overcurrent.

[0257] In this embodiment, the core parameters involved in the path planning and control adjustment process are shown in the table below. By fixing the values ​​of the above parameters, it is ensured that the travel trajectory generated by the path planning conforms to the kinematic constraints of the robot. At the same time, the adjustment process of the drive wheel torque and the fan power can meet the control requirements under different turning conditions.

[0258] Table 4 Core Parameters for Path Planning and Control Regulation

[0259]

[0260] Among the parameters defined in this table, the chassis wheelbase and maximum steering angle are fixed parameters of the robot's mechanical structure, while the remaining parameters are configurable parameters that can be adjusted according to factors such as the robot's operating scenario, motion performance, and motor characteristics.

[0261] This embodiment optimizes the fusion process of the comprehensive cost map by using a piecewise linear slip penalty function and dynamic weight coefficients, enabling the cost map to dynamically adjust the weight of dust concentration distribution according to the magnitude of slip risk. By introducing a minimum turning radius constraint to correct the direction vector of path node expansion, it ensures that the generated trajectory conforms to the robot's kinematic constraints. The drive wheel torque adjustment under turning conditions is optimized through a PID control algorithm. At the same time, the centrifugal acceleration estimate is accurately calculated by combining the installation position of the fan, and the fan power adjustment process is optimized by introducing a motor overcurrent protection mechanism, further improving the robot's path tracking accuracy and operational stability in harsh high-dust environments.

Claims

1. A method for robot cleaning control and path planning in harsh environments, characterized in that, include: The robot's drive wheel speed signal and the real-time negative pressure value of the cleaning fan are acquired in real time. The real-time negative pressure value is converted into a fan load torque variable, and the fan load torque variable and the drive wheel speed signal are input into a preset chassis slip ratio calculation model to obtain the chassis slip ratio at the current moment. When the chassis slip ratio is greater than the preset slip ratio threshold, the negative pressure change rate of the real-time negative pressure value at the current moment is extracted as the dust concentration feature, and a dust concentration gradient field with the current coordinate point as the starting point is constructed by combining the robot's historical pose data. The chassis slip ratio is fused with the dust concentration gradient field to generate a comprehensive cost map; The travel trajectory is generated by a path planning algorithm under the constraint of reducing the slip ratio cost in the comprehensive cost map and moving along the gradient descent direction of the dust concentration gradient field. Based on the curvature of the travel trajectory, the torque of the robot's drive wheels and the power of the cleaning fan are adjusted.

2. The method according to claim 1, characterized in that, The real-time negative pressure value is converted into a fan load torque variable, and the fan load torque variable and the drive wheel speed signal are input to a preset chassis slip ratio calculation model to obtain the chassis slip ratio at the current moment, including: obtaining the current speed of the cleaning fan, and using the ratio of the real-time negative pressure value to the current speed as the aerodynamic drag coefficient; The aerodynamic drag coefficient is multiplied by the square of the current rotational speed, and combined with the preset mechanical friction compensation coefficient, to calculate the fan load torque variable. The drive wheel speed signal is converted into drive wheel linear velocity, and the fan load torque variable is introduced as a disturbance term into the robot's chassis dynamics equation. The theoretical drive wheel linear velocity is obtained by solving the chassis dynamics equation. The chassis slip ratio at the current moment is obtained by dividing the difference between the theoretical drive wheel linear velocity and the drive wheel linear velocity by the theoretical drive wheel linear velocity.

3. The method according to claim 2, characterized in that, Extract the negative pressure change rate of the real-time negative pressure value at the current moment as a dust concentration feature, and construct a dust concentration gradient field with the current coordinate point as the starting point by combining the robot's historical pose data. This includes: truncating the real-time negative pressure value with a fixed time window, and performing first-order difference calculation on the truncated negative pressure time series to obtain the negative pressure change rate. Retrieve the coordinate sequence from the historical pose data corresponding to the fixed time window; Each coordinate point in the coordinate sequence is taken as a spatial node, and the negative pressure change rate corresponding to each spatial node is mapped to the dust concentration estimate of the spatial node. Centered on the current coordinate point, a preset spatial interpolation algorithm is used to interpolate the estimated dust concentration values ​​of adjacent spatial nodes to generate a dust concentration gradient field containing spatial distribution information.

4. The method according to claim 3, characterized in that, The process of fusing the chassis slip ratio with the dust concentration gradient field to generate a comprehensive cost map includes: obtaining the dust concentration estimate corresponding to each grid cell in the dust concentration gradient field in a preset basic occupancy grid map; The dust concentration estimate is input into a preset slip penalty function to calculate the slip penalty value for each grid cell, wherein the slope of the slip penalty function is positively correlated with the chassis slip ratio. Obtain the basic passage cost of the free grid cells in the basic occupied grid map; The basic passage cost value of each free grid cell is weighted and summed with the corresponding slip penalty cost value. The summation result is used as the comprehensive cost value of the free grid cell in the comprehensive cost map. The comprehensive cost value of the obstacle grid cell is set to infinity.

5. The method according to claim 4, characterized in that, With the constraint of reducing the slip ratio cost in the comprehensive cost map and moving along the gradient descent direction of the dust concentration gradient field, a path planning algorithm is used to generate a travel trajectory, including: taking the robot's current coordinate point as the starting node, and determining the set of adjacent extended nodes of the starting node in the comprehensive cost map. Calculate the direction vector of the line connecting the starting node to each node in the set of adjacent extended nodes; Calculate the negative gradient direction vector of the starting node in the dust concentration gradient field; Calculate the cosine of the angle between each of the connecting direction vectors and the negative gradient direction vector, and add the cosine of the angle to the comprehensive cost value of the nodes in the adjacent extended node set to obtain the evaluation cost value; The node with the lowest evaluation value is selected as the target child node, and the expansion process is repeated until the termination condition is met. The path connecting all target child nodes is taken as the travel trajectory.

6. The method according to claim 5, characterized in that, Adjusting the drive wheel torque of the robot and the fan power of the cleaning fan based on the trajectory curvature of the travel trajectory includes: discretizing the travel trajectory to obtain a sequence of trajectory points composed of multiple discrete trajectory points, and calculating the trajectory curvature corresponding to each discrete trajectory point; When the trajectory curvature of the discrete trajectory point is greater than a preset curvature threshold, the current turning angular velocity of the robot is obtained, the torque adjustment amount is calculated based on the difference between the current turning angular velocity and the trajectory curvature, and the torque adjustment amount is superimposed on the reference value of the drive wheel torque. The estimated centrifugal acceleration on the side where the cleaning fan is located is calculated based on the trajectory curvature. The product of the fan power and the estimated centrifugal acceleration is used as the power suppression parameter. The initial power of the cleaning fan is subtracted from the power suppression parameter to obtain the final output power.

7. The method according to claim 6, characterized in that, The aerodynamic drag coefficient is multiplied by the square of the current rotational speed, and combined with the preset mechanical friction compensation coefficient to calculate the fan load torque variable, including: real-time acquisition of the three-phase current signal of the fan drive motor driving the cleaning fan, performing Clark transformation and Park transformation on the three-phase current signal, and extracting the d-axis current component and q-axis current component. Multiply the q-axis current component by the torque constant of the cleaning fan to obtain the measured output torque; The theoretical aerodynamic load torque is obtained by multiplying the aerodynamic drag coefficient by the square of the current rotational speed. The difference between the measured output torque and the theoretical aerodynamic load torque is used as the dynamic friction torque observation value. The dynamic friction torque observation value is filtered using a low-pass filter, and the filtering result is used as the updated mechanical friction compensation coefficient in the calculation.

8. The method according to claim 7, characterized in that, Taking each coordinate point in the coordinate sequence as a spatial node, and mapping the negative pressure change rate corresponding to each spatial node to the dust concentration estimate of the spatial node, includes: synchronously acquiring the brush rotation speed signal of the robot's cleaning brush within the fixed time window; Extract the continuous high-speed periods in the brush rotation speed signal that are greater than a preset speed threshold, and mark the spatial nodes in the coordinate sequence corresponding to the continuous high-speed periods as effective disturbance nodes; For the effective disturbance node, the ratio of the negative pressure change rate to the brush rotation speed signal at the corresponding time is calculated as the standardized concentration feature; For non-effective perturbation nodes, the standardized concentration feature is reduced according to the time decay factor; The standardized concentration characteristics of the effective perturbation nodes and the reduced characteristics of the ineffective perturbation nodes are used as the dust concentration estimates for the corresponding spatial nodes.

9. The method according to claim 8, characterized in that, Calculate the cosine of the angle between each of the connecting direction vectors and the negative gradient direction vector, and add the cosine of the angle to the comprehensive cost value of the nodes in the adjacent extended node set to obtain the evaluation cost value, including: obtaining the chassis wheelbase and maximum steering angle of the robot, and calculating the minimum turning radius of the robot; Determine whether the distance from the starting node to a node in the set of adjacent extended nodes is less than the minimum turning radius; If it is less than the minimum turning radius, then the connecting direction vector is projected onto the tangent direction of the arc with the starting node as the center and the minimum turning radius as the radius to obtain the corrected direction vector; Calculate the cosine of the angle between the corrected direction vector and the negative gradient direction vector, and add the reciprocal of the distance as a distance penalty term to the calculation of the evaluation cost value, replacing the original comprehensive cost value for addition calculation.

10. The method according to claim 9, characterized in that, The estimated centrifugal acceleration on the side where the cleaning fan is located is calculated based on the trajectory curvature, and the product of the fan power and the estimated centrifugal acceleration is used as a power suppression parameter, including: obtaining the installation offset distance and installation offset angle of the robot's cleaning fan in the vehicle coordinate system; The current lateral acceleration of the robot is calculated based on the trajectory curvature of the discrete trajectory points and the robot's current travel velocity. The current lateral acceleration is decomposed into the direction corresponding to the installation offset angle, and the estimated centrifugal acceleration of the sweeping fan is calculated by combining the installation offset distance. The q-axis current component of the fan drive motor as described in claim 7 is retrieved. When the q-axis current component is greater than the preset overcurrent threshold, the power suppression parameter is multiplied by a forced attenuation coefficient greater than 1 and then subtracted from the initial power to limit the lower limit of the fan power to not be lower than the minimum power threshold for maintaining negative pressure.

Images