Disclosure of Invention
The invention aims to provide a method for judging robot dyskinesia by adopting a two-stage filtering algorithm and a peak filtering algorithm.
The invention provides a method for judging dyskinesia of a robot, which comprises the following steps:
s1: collecting a plurality of groups of current data values of an analog-to-digital conversion circuit of a motor for driving the robot to move;
s2: calculating the average value of the currents of the "multiple sets of current data values" of the step S1;
s3: calculating the number of the current larger than the average value of the current;
s4: if the quantity larger than the current average value is smaller than the preset quantity, changing the current data value larger than the current average value into the current average value, and recalculating the current average value;
s5: initializing a historical current average value, and storing the current average value into the historical current average value;
s6: calculating an average value of historical current average values;
s7: if the current average current value is larger than a preset value and the difference between the current average current value and the average value of the historical current average value is larger than the minimum value of the preset difference value, the fact that an obstacle is met is inferred, and in order to avoid repeated judgment, the original current average value is completely replaced by a new average value; otherwise, judging that no obstacle is encountered and returning to the step S1 to judge again; otherwise, it is determined that no obstacle is encountered and the process returns to step S1 to make the determination again.
Further, step S1 reads the current data of the analog-to-digital conversion circuit of the motor in the DMA manner.
Further, in step S2, the average value of the current of the "current data" is calculated by an arithmetic method.
Further, in step S2, the average current value is calculated by subtracting the maximum value of the "current data value" and subtracting the minimum value of the "current data value".
Further, the meaning of "changing the current data value larger than the current average value to the current average value, recalculating the current average value" in step S4 is: the current data values larger than the current average value are changed to the current average value a, a and the current average value is calculated together with the circuit data values smaller than the current average value.
Further, with respect to step S4, if the number larger than the current average value is equal to or larger than the preset number, step S5 is "save the present current average value as the history current average value".
Further, in step S7, if the current average value is larger than the predetermined value, it represents that an obstacle may be encountered.
Further, in step S7, "if the current average value is larger than the preset value and is larger than" the average value of the historical current average values "by a given value", it represents that the current value "rises significantly".
Further, the step S1 adopts an algorithm avoiding floating point operation.
Further, the number of the current data amount collected at step S1 and the historical current average value at step S5 are both N powers of 2.
The invention adopts a multistage filtering algorithm, and judges whether the window cleaning robot meets obstacles or not by analyzing the current change; although the algorithm is complex and needs more experiments to determine the relevant parameters, the whole structure of the robot is simplified and the wiring is greatly reduced because no detection switch is needed, thereby greatly reducing the whole failure rate.
Detailed Description
In order to make the objects, technical solutions and advantages of the present invention more clearly understood, the following describes an intelligent following exercise device in detail with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative of the invention and are not intended to limit the invention.
The invention relates to a method for judging dyskinesia of a robot, which is used for a window cleaning robot and other walkable robots.
The window cleaning robot comprises two crawler belt driving motors moving by a driver and a fan motor controlling automatic window cleaning, when the window cleaning robot encounters an obstacle in the window cleaning process, the load of the driving motors is increased, the current of the driving motors is obviously increased, various interferences are filtered by reading the data of the current and adopting a multistage filtering algorithm, and whether the current data is obviously increased or not is analyzed. If a "significant rise" occurs, it represents that the window-cleaning robot has encountered an obstacle.
The window cleaning robot has loads when walking, the loads of walking in different directions are different, current data are interfered, and how to define 'obvious rising' is difficult, so that the algorithm is complex, and the problem is solved by a multi-stage filtering algorithm, as shown in figure 1.
Collecting a plurality of groups of current data values of an analog-to-digital conversion circuit of a motor for driving the window cleaning robot to move in a DMA mode; calculating the average value of the current; calculating the data volume larger than the current average value, if the data volume is too small and smaller than the preset number, representing that a peak interference value exists, replacing the interference value with the average value, and recalculating the average value; initializing a historical average value, and storing a current average value into the historical average value; calculate "average of historical averages"; if the current average is greater than a given value (representing a possible encounter with an obstacle) and greater than the "average of historical averages" by a given value (representing a "significant rise" in current value), then concluding that an obstacle has been encountered; to avoid repeated judgments, the original average values are all replaced by new average values.
A method for judging robot dyskinesia comprises the following steps:
s1: collecting a plurality of groups of current data values of an analog-to-digital conversion circuit of a motor for driving the robot to move;
s2: calculating the average value of the currents of the "multiple sets of current data values" of the step S1;
s3: calculating the number of the current larger than the average value of the current;
s4: if the quantity larger than the current average value is smaller than the preset quantity, the peak interference value is represented, the current average value is adopted to replace the interference value, the current data value larger than the current average value is changed into the current average value, and the current average value is recalculated;
s5: initializing a historical current average value, and storing the current average value into the historical current average value;
s6: calculating an average value of historical current average values;
s7: if the current average current value is larger than a preset value and the difference between the current average current value and the average value of the historical current average value is larger than the minimum value of the preset difference value, the fact that an obstacle is met is inferred, and in order to avoid repeated judgment, the original current average value is completely replaced by a new average value; otherwise, judging that no obstacle is encountered and returning to the step S1 to judge again; otherwise, it is determined that no obstacle is encountered and the process returns to step S1 to make the determination again.
In step S1, current data of the analog-to-digital conversion circuit of the motor is read in a DMA manner.
The DMA method, also called a group data transfer method and sometimes called a Direct Memory operation, does not save a field, restore a field, or the like during data transfer.
In step S2, the arithmetic method is used to calculate the average current value, or the filtering algorithm that takes the average current value by the median method, or the average current average value by the sliding average method, or the average current average value by the weighted sliding average value instead of the arithmetic average filtering algorithm.
The phrase "changing the current data value larger than the current average value to the current average value, recalculating the current average value" in step S4 means that: the current data values larger than the current average value are changed to the current average value a, a and the current average value is calculated together with the circuit data values smaller than the current average value.
For step S4, if the number larger than the current average value is equal to or larger than the preset number, step S5 is "save the current average value as the history current average value".
The "storing the current average value into the history current average value" of step S5 means that the recalculated and obtained current average value of step S4 is taken as the history current average value.
The "calculating an average value of the historical current average" in step S6 means that the average value of the historical current average values is calculated based on the plurality of sets of historical current average values obtained by the methods in steps S1 to S5.
"if the current average value I1 is greater than the preset value I2 and the difference between the current average value I1 and the historical current average value I3 is greater than the preset minimum value I4 of the difference, i.e., I1 > I2, I1-I3 > I4" of step S7.
Wherein in step S7, if the current average value is greater than the predetermined value, it represents that the window-cleaning robot may encounter an obstacle. In step S7, "if the current average value is greater than the preset value and the difference between the current average value and the historical current average value is greater than the minimum value of the preset difference value," the current value "rises significantly".
In step S7, the "original current average value" indicates that a batch of current average values are stored in the present determination method, and the "new current average value" indicates the current average value.
The window cleaning robot travels in different directions, so that the load change is large, and the current value change is large; and the circuit itself has large interference, which also causes the current to change dramatically. Therefore, the invention adopts two-stage average filtering and superposition peak interference removing filtering (namely, the two previous stages are arithmetic average filtering algorithms, and the peak interference removing algorithm replaces the peak value with the average value), and the problems can be completely solved as long as the parameters are properly selected.
The invention also adopts an algorithm avoiding floating point operation (namely, floating point operation is not needed, the floating point operation speed is much slower than that of the common operation, so that in a place with higher real-time requirement, if the floating point operation is needed, an FPU (floating point operation unit) is added, and the cost is much higher), wherein the algorithm avoiding the floating point operation is adopted in the step S1.
Floating-point operations are real operations, which are slow and subject to errors. Most machines now are 32-bit, that is, if 32 bits are used to represent integers, then 0 to 2^32-1 for unsigned integers and-2 ^31 to 2^31-1 for signed integers.
The invention can be operated on a common singlechip quickly. The acquired AD data are not converted into the current value of floating point number, and the integer operation is directly adopted; the number of the current data amounts collected in step S1 and the number of the historical current average values in step S5 are both N powers of 2 (N powers +2 of 2 if the maximum and minimum values are to be discarded), and the average values are calculated by a binary shift operation.
The binary system is right-shifted by 1 bit, namely, by 2, right-shifted by 2 bits, namely, by 4, and right-shifted by N bits, namely, by the power of N of 2. Therefore, after the data size is designed to be the power of N of 2, only N bits need to be shifted to the right, and the division is finished and is originally calculated by floating point.
The invention adopts an arithmetic mean value filtering algorithm and a filtering algorithm for removing peak-peak interference. The arithmetic mean filtering adopted by the invention is a simple arithmetic mean, can be replaced by removing the maximum and minimum values and then calculating the mean, or filtering algorithms such as median, median average, sliding mean, weighted sliding mean and the like are adopted to replace the arithmetic mean filtering algorithm. The filtering algorithm for eliminating the peak interference is to replace the peak with the average value, and can also calculate the average value after simply removing a plurality of peak values.
The robot driving device judges whether an obstacle is encountered or not by collecting current data of a motor driving the robot to move and detecting the change of current; the invention adopts a two-stage filtering algorithm of 'average value' + 'average value of the average value' and then superposes a filtering algorithm for removing peak to judge whether the current is 'remarkably increased'; the two-stage filtering algorithm adopted by the invention can be popularized to multiple stages according to the needs to adapt to more complex interference environments; each stage of filtering algorithm can flexibly adopt various mature filtering algorithms; floating-point operations are avoided, the collected data volume is the power N of 2 (or the power N +2 of 2), and binary shifting operations are used for replacing the floating-point operations.
The invention adopts a multistage filtering algorithm and analyzes the current change to judge whether the window cleaning robot meets the obstacle. Although the algorithm is complex and more experiments are needed to determine relevant parameters, the overall structure of the robot is simplified and the wiring is greatly reduced because no detection switch is needed, so that the overall failure rate is greatly reduced.
Although the present invention has been described with reference to a preferred embodiment, it should be understood that various changes, substitutions and alterations can be made herein without departing from the spirit and scope of the invention as defined by the appended claims.