Summary of the invention
The purpose of the present invention is providing a kind of robot motion using two-stage grade filtering algorithm and the filtering algorithm of spike
The judgment method of obstacle.
The present invention provides a kind of judgment method of robot motion's obstacle, includes the following steps:
S1: the multiple groups current data value of the analog to digital conversion circuit of the motor of acquisition driving robot motion;
S2: the current average of " the multiple groups current data value " of step S1 is calculated;
S3: the quantity for being greater than current average is calculated;
S4: if the quantity bigger than current average is less than preset quantity, it will be greater than the electric current of current average
Data value is changed to current average, recalculates current average;
S5: current flow average value is stored in historical current average value by initialization historical current average value;
S6: it calculates " average value of historical current average value ";
S7: if current flow average value is greater than preset value and current flow average value and " historical current average value
Average value " difference be greater than preset difference minimum value, then infer encounter obstacle, for avoid repeat judge, will be former
The current average come is all substituted for new average value;Otherwise judgement, which is encountered obstacle and returns to step S1 and re-start, sentences
It is disconnected;Otherwise judgement, which is encountered obstacle and returns to step S1, re-starts judgement.
Further, step S1 reads the current data of the analog to digital conversion circuit of motor using dma mode.
Further, the current average of " current data " is calculated in step S2 using the method that counts.
Further, it is used in step S2 and removes the maximum value of " current data value " and remove the minimum of " current data value "
Calculating current average value again after value.
Further, in step S4 " current data value that will be greater than current average is changed to current average, counts again
Calculate current average " mean that the current data value greater than current average be changed to current average be a, a and be less than electric current
The circuit data value of average value calculating current average value together.
Further, for step S4, if the quantity bigger than current average is greater than preset quantity, step
S5 is " saving current flow average value as historical current average value ".
Further, wherein in step S7, if current flow average value is greater than preset value, representative is likely encountered barrier
Hinder.
Further, in step S7, " if current flow average value is greater than preset value and than " historical current is average
The average value of value " big certain given value ", represents current value " significantly rising ".
Further, the step S1 uses the algorithm for avoiding floating-point operation
Further, the quantity of the historical current average value of the current data amount and step S5 of step S1 acquisition is all 2
Nth power.
The present invention uses multilevel filter algorithm, by analyzing curent change, to judge whether window wiping robot meets
To obstacle;Although algorithm of the invention is complicated, and more experiment is needed to determine relevant parameter, because not needing any detection
Switch, simplifies the overall structure of robot, substantially reduces wiring, to substantially reduce overall failure rate.
Specific embodiment
In order to make the objectives, technical solutions, and advantages of the present invention clearer, with reference to the accompanying drawings and embodiments, right
A kind of intelligence of the present invention follows telecontrol equipment to be described in further detail.It should be appreciated that specific embodiment described herein is only
To explain the present invention, it is not intended to limit the present invention.
A kind of judgment method of robot motion's obstacle of the present invention, is used for window wiping robot, can be used for other can
With the robot of walking.
Window wiping robot includes the blower motor of the two track drive motors and control automatic window-cleaning of driver movement, when
When window wiping robot encounters obstacle during wiping window, the load that will cause driving motor increases, and leads to the electric current of driving motor
It dramatically increases, by reading the data of electric current, various interference is filtered using multilevel filter algorithm, whether analysis current data is " aobvious
It writes and rises ".If there is " significantly rising ", then represents window wiping robot and encounter obstacle.
Per se with load when due to window wiping robot walking, the load of different directions walking is different, and current data
Itself have interference, how to define " significantly rising " it is relatively difficult, cause algorithm complicated, the present invention passes through multilevel filter algorithm solution
Certainly this problem, as shown in Figure 1.
The multiple groups electric current number of the analog to digital conversion circuit of the motor of acquisition driving window wiping robot movement is passed through using dma mode
According to value;Calculating current average value;The big data volume of calculating ratio current average again, if very little and less than preset number
Amount, then representing has spike interference value, just replaces interference value with average value, and recalculate average value;History average is initialized,
Current average is stored in history average;It calculates " average value of history average ";If the big Mr. Yu of current average is given
Value (representative is likely encountered obstacle) and certain given value (representing current value " significantly rising ") bigger than " average value of history average ",
Then infer and encounters obstacle;To avoid repeating judging, original average value is all substituted for new average value.
A kind of judgment method of robot motion's obstacle, includes the following steps:
S1: the multiple groups current data value of the analog to digital conversion circuit of the motor of acquisition driving robot motion;
S2: the current average of " the multiple groups current data value " of step S1 is calculated;
S3: the quantity for being greater than current average is calculated;
S4: if the quantity bigger than current average is less than preset quantity, representative has spike interference value, and adopts
Interference value is replaced with current average, the current data value that will be greater than current average is changed to current average, recalculates electricity
Levelling mean value;
S5: current flow average value is stored in historical current average value by initialization historical current average value;
S6: it calculates " average value of historical current average value ";
S7: if current flow average value is greater than preset value and current flow average value and " historical current average value
Average value " difference be greater than preset difference minimum value, then infer encounter obstacle, for avoid repeat judge, will be former
The current average come is all substituted for new average value;Otherwise judgement, which is encountered obstacle and returns to step S1 and re-start, sentences
It is disconnected;Otherwise judgement, which is encountered obstacle and returns to step S1, re-starts judgement.
Wherein, step S1 reads the current data of the analog to digital conversion circuit of motor using dma mode.
Dma mode, Direct Memory Access, also referred to as burst data transmission mode are otherwise referred to as directly interior
Operation is deposited, dma mode is in data transfer procedure, without the work of saving scene, restoring scene etc.
It is the maximum value that " current data value " is removed in calculating current average value or use that the method for counting is used in step S2
With remove " current data value " minimum value after again calculating current average value or using median method obtaining current average value or
The average obtaining current average value or sliding average method obtaining current average value or weighted moving average of person's median take
The averagely equal filtering algorithms of electric current replace arithmetic mean of instantaneous value filtering algorithm.
In step S4 " current data value that will be greater than current average is changed to current average, and it is flat to recalculate electric current
Mean value " to mean that the current data value greater than current average is changed to current average be a, a and less than current average
Circuit data value calculating current average value together.
For step S4, if the quantity bigger than current average is greater than preset quantity, step S5 is " to save
Current flow average value is as historical current average value ".
" current flow average value is stored in historical current average value " of step S5 means the meter again of step S4
It calculates and obtained current average is as historical current average value.
The average value of " calculating " historical current average value in step S6 " " it means, according to the method for step S1 to S5
The multiple groups historical current average value of acquirement, then calculate the average value of multiple groups historical current average value.
Step S7 " if current flow average value I1 is greater than preset value I2 and current flow average value I1 and history
The difference of the average value I3 of current average is greater than the minimum value I4, i.e. I1 > I2, I1-I3 > I4 of preset difference.
Wherein in step S7, if current flow average value is greater than preset value, represents window wiping robot and be likely encountered
Obstacle.In step S7, " if current flow average value is greater than preset value and current flow average value and historical current are average
The difference of the average value of value is greater than the minimum value of preset difference ", represent current value " significantly rising ".
" current average originally " refers to saving a collection of current average in this judgment method in step S7, " new
Current average " refer to current current average.
Since window wiping robot is walked toward different directions, load is changed greatly, and current value changes greatly;And circuit itself
Interference is big, will also result in the more violent variation of electric current.Therefore, from present invention employs two-stage average value filterings to be superimposed deglitch
(i.e. front two-stage is all arithmetic mean of instantaneous value filtering algorithm to value interference filter, and deglitch value-based algorithm is exactly to replace spike with average value
Value), as long as parameter selection is proper, it can solve these problems completely.
The present invention, which additionally uses, to be avoided the algorithm of floating-point operation and (that is: not to need to carry out floating-point operation, floating-point operation is than common
Arithmetic speed it is many slowly, so FPU (floating-point can be added if necessary to floating-point operation to the higher place of requirement of real-time
Arithmetic element), cost can be much higher.), wherein step S1 uses the algorithm for avoiding floating-point operation.
Floating-point operation is exactly real arithmetic, and floating-point operation is very slow and has error.Most of machines are all now
32, that is to say, that if 32 are all used to indicate integer, then being exactly 0 to 2^32-1 for signless integer, for having
It is exactly -2^31 to 2^31-1 if symbol.
The present invention can quickly be run on common single-chip microcontroller.The AD data of acquisition are not converted into the current value of floating number,
Directly adopt integer arithmetic;The quantity of the historical current average value of the current data amount and step S5 of step S1 acquisition is all 2
Nth power (if to remove Nth power+2 that is maximum, changing 2 if minimum value into), calculated by binary shifting function average
Value.
Wherein, binary shift right 1, it is exactly to move to right N divided by 4, be exactly divided by 2 that practical, which is exactly to move to right 2 divided by 2,
Nth power.So only need to move to right N after data volume to be designed to 2 Nth power, just complete division, division is floating-point originally
Operation.
The present invention is using arithmetic mean of instantaneous value filtering algorithm and deglitch value interference filter algorithm.What the present invention used
Arithmetic average value filtering is simple arithmetic mean of instantaneous value, calculates average value again after being substituted for maximin, or use
The filtering algorithms such as median, median be average, sliding average, weighted moving average replace arithmetic mean of instantaneous value filtering algorithm.
Deglitch value interference filter algorithm is to replace kurtosis with average value, is calculated again averagely after can also simply removing several kurtosis
Value.
The present invention drives the current data of the motor of robot motion, then the variation by detecting electric current to sentence by acquisition
It is disconnected whether to encounter obstacle;Present invention employs " average value "+" average value of average value " two-stage grade filtering algorithms, then are superimposed point
The filtering algorithm at peak, whether to judge electric current " significantly rising ";The two stage filter algorithm that the present invention uses, which can according to need, to be pushed away
It is wide to arrive multistage, deacclimatize more complicated interference environment;Every level-one filtering algorithm can be flexibly using a variety of mature filtering algorithms;It keeps away
Exempt from floating-point operation, the data volume of acquisition is 2 Nth power (or 2 Nth power+2), replaces floating-point to transport with binary shifting function
It calculates.
The present invention uses multilevel filter algorithm, by analyzing curent change, to judge whether window wiping robot meets
To obstacle.Although algorithm is complicated, and needs more experiment to determine relevant parameter, because not needing any detection switch,
The overall structure for simplifying robot substantially reduces wiring, to substantially reduce overall failure rate.
The above described is only a preferred embodiment of the present invention, be not intended to limit the present invention in any form, though
So the present invention has been disclosed as a preferred embodiment, and however, it is not intended to limit the invention, any technology people for being familiar with this profession
Member, without departing from the scope of the present invention, when the technology contents using the disclosure above are modified or are modified
It is right according to the technical essence of the invention for the equivalent embodiment of equivalent variations, but without departing from the technical solutions of the present invention
Any simple modification, equivalent change and modification made by above embodiments, all of which are still within the scope of the technical scheme of the invention.