One idea that’s been banging around in my head ever since our team successfully switched to closed-loop drive control is using the feedback not only to correct for, but also to detect, changes in the mechanical behavior of a system
For example, at some point during competition this year, we blew a bearing on one of our drive axles. Our closed-loop control was able to correct for this well enough that we did not notice it for quite a while. But, by that same token, it seems to me that the robot ought to be able to have told us that something was wrong. Similarly, we had problems with our motors being pushed from the “inboard” to the “outboard” position in our WCP gearboxes - this, too, should have been detectable.
The basic idea I have involves looking at the “non-feedforward” portion of the motor output. It seems to me that this value should, on balance, increase in the case of a failure that increases friction, as the control loop increases output to compensate. Thus, if we can establish a baseline value for this, the robot should be able to tell us if it increases above baseline, indicating the presence of some sort of mechanical fault.
This isn’t trivial, however - some degree of filtering will almost certainly be required, for starters. Moreover, while I think this approach likely should work well in situations when the motors are being given achievable commands (e.g. a drive when following a reasonable motion profile), a robot drive during actual teleoperated control likely has lots of transients when the setpoint rapidly changes and I’m unsure if this method would be robust enough to work in such a situation, even with filtering.
Has anyone done anything like this, and if so, do you have any advice? Else, does this seem like a fundamentally sound/worth-pursuing idea?
Here’s what I would try (based partially off of the super-crude fault detection code I write for assembly lines at work):
Look for some sort of output vs. input comparison. Like say output > 125% of input or the like
Then put in a timer to filter out momentary bursts as expected with closed loop control
This approach can also be used for non-PID things that have limit switches and/or tachmometers.
For fixed-speed things (pnematics, motorized things on relays, etc.) just set up a timer that activates when a motion is commanded, and if you don’t see the limit switch/tach signal in enough time, that’s a fault condition.
For variable things, the PID approach in the first two points would also work here, as you’d need to account for % commanded to know what time to look for.
The third point is what 99% of the code I write at work is, as it’s almost entirely pneumatics with cylinder switches (or other position switches) and fixed/ selectable speed motors. In one unique case I had a machine that was used to separate brackets for attaching courtesy handles to cars (you know, the handles on the headliner of cars and trucks) that used two twin-speed DC motorized magnet bars, in which case all I did for fault detection was make a timer that reset every time a leading edge of one of the position sensors tripped (it had multiple sensors as it was on a vertical slide to lift brackets out of a bucket/hopper), and if that timer ever went above 5 seconds (low speed) or 2 seconds (high speed), set the fault for a jammed/grinding motor drive. Doing so on PLCs/Ladder Logic (what all my fault detection code is written to/in, yay automotive subassembly industry) is easy… in text/labview it’s a bit tricker but by no means difficult.
The whole point here is to make it both effective and lean, as ideally you’ll be running multiple instances, one for each device (and direction in some cases). If you’re going to fault detection, and you make it work, why not do it everywhere?
We implemented a motor sanity check on all our drive motors that went like this:
Apply a low voltage to said motor and record speed and current.
Apply a high voltage to said motor and record speed and current.
Build a trend (graph the info).
The first set of measurements is when everything is new and fresh. If things start turning badly then these measurements will change. Using this method, we could predict RS775 failures. You could use just speed or just current which should detect a mechanical issue however in the case of the 775 failure, both were needed because the speed step changed but the current stayed within the norms.
No method is fool proof but this method should have caught a bad bearing issue.
I should mention this is test code run in the pits after every match.
I haven’t done this, but in the case of an FRC robot, it may make more sense to do such fault detection between matches (from log files), if you’re thinking of anything more than noting the application of large corrections. As long as the software is compensating in real-time, there’s not much you can do during a match.
We’d definitely primarily be interested in writing detected faults to our logs, but it could also be helpful to inform the drivers if the robot thinks something is wrong (“hey, that big collision seems to have broken something, we should be ready to compensate if the robot starts driving poorly”).
A good way to do this is to look at expected current draw vs actual. Although we didn’t do this on board the robot this year, I repeatedly found dying motors this way by analysing post-match logs, and you’d see a similar current draw for bearings slipping/parts rubbing/etc.
In terms of doing fault detection with encoders, something that I’ve thought about is using a simulated model of the system running on the robot, and comparing the simulated model’s velocity to the actual velocity (Ideally, you’d compare the positions, but it’s likely that there would be too much drift there). I’ve never tested this exact setup, but we did something similar (although easier) in order to detect encoder failures this year. Downside is that your model has to at least approximately match reality in order for this to work.
I’m not sure how much practical benefit there is in informing the driver of failures during the match, but if it’s something that your programming team is interested in and capable of it’s definitely a fun problem to work on
This is essentially what looking at the “non-feedforward” portion of motor voltage is intended to do - in a velocity loop, the feedforward term is a first-order approximation of the voltage required to achieve the setpoint. Thus, the amount of additional output required on top of the feedforward term corresponds to the deviation of the system from the expected behavior. If the system becomes damaged (unless the damage somehow *improves *performance), this should increase (note that care has to be taken regarding the sign, here, which must be relative to the desired ouput direction - while at some speeds, the feedforward output will be higher than the output required to reach the setpoint, resulting in a negative “non-feedforward output” term, damage to the system would still cause this term to “increase” for such speeds, even if such an “increase” takes the form of a decrease in magnitude of the negative term).
Of course, this has its own share of problems - firstly, the approximation is for steady-state, and teleoperated driving is very rarely steady-state (or even quasi-steady-state). Additionally, motor response isn’t perfectly linear, and so even at steady-state the amount of correction applied after feedforward is going to vary by setpoint even in an undamaged system; as noted earlier, for a correctly-chosen feedforward term, the “non-feedforward output” at steady-state will positive for some speeds, and negative for others (if it’s positive for all speeds, kF should clearly be increased). Thus, some change is likely to be seen just from driving at different speeds - whether this effect is small enough, in the case of ordinary driving, for this approach to work remains to be seen.
I suspect clever filtering might help account for some of these problems, but am unsure how well it will ultimately work in practice. Still, I think it’s probably worth pursuing.
Ultimately, it may be that the most fruitful approach is a fusion of the different ideas mentioned here.
The way I’ve done this before is to hook everything up to a kalman filter and estimate the disturbance force. We used that method this year to detect when our indexer jammed on a ball.
If your model of your system is
X(n + 1) = A X(n) + B U(n)
then you can augment that to add the disturbance force.
Assuming a specific operating point on your motor (half speed), if you increase the voltage by delta V, you will expect to see a change in current of delta I. Current is proportional to torque, which is proportional to the external force. So, you can model disturbance forces as voltages.
Over time, U_disturbance will converge to the estimated control input required to remove the bias from your model. Or, to put it another way, it will converge to the unmodeled forces. Like failed bearings, balls stuck in the mechanism, walls, other robots, etc. You can pick how fast you want to learn disturbances. Faster -> noisier, slower -> smoother. I also find that the disturbance estimate is a good place to go looking to see how well your model matches reality.
I always prefer to split the world up into an observer/kalman filter and a controller. The observer’s job is to sort out what’s going on in the world. For example, you want to estimate the position, velocity, and disturbance force on your mechanism. You also want to smooth out noisy sensor measurements with a model. The controller’s job is to then take the estimate of the world and figure out what to do about it.
If you are interested, X_hat(4, 0) and X_hat(5, 0) are the estimated disturbance forces in 971’s drive code. We use this to implement integral control (ie, compensating for disturbance forces)**
