Motor controller with feed-forward for Lego NXT

Goals
During experiments with Java version of Lego motor controller and built-in LeJOS motor controller I found that they use different approaches to implement smooth acceleration. Lego controller uses integer arithmetics and implements non-linear acceleration algorithm. It divides the acceleration distance to the speed delta and gets the interval to increase speed for 1 degree/sec. Each time when the motor covers the interval, the controller increases its speed for 1 degree/sec. This algorithms is simple but acceleration is non-linear. LeJOS uses different approach. The version I tested had problems with smooth acceleration. But LeJOS team has published an updated version of motor controller that works well. This version implements linear acceleration approach using floating point arithmetic. Also LeJOS controller uses much smalled discretization interval (interval between regulation cycles). Lego controller uses 100ms interval, LeJOS controller uses 4ms interval. This small regulation interval has some advantages and disadvantages: it provides more fast reaction to change of external load but requires more CPU time, especially being implemented in Java. Lego controller is implemented in native ARM code and works faster.

I wanted to make a controller that has best qualities of LeJOS controller but requires less CPU time. I tried to increase discretization interval but found that this controller has problems with rotation to a specified angle. It could not reach the target position with zero speed and requires additional time to reach the target point.


To implement smooth acceleration, I used motion profile generator that calculated the next position at each regulation cycle and PID regulator that should keep the specified position. This regulator works well when the set point changes slowly. But when we use smooth acceleration, the set point is changed each regulation cycle and the regulator has some constant delay in reaching the target position. As as result, the motor comes in the target rotation position with non-zero speed and spends some time for oscillations around the target point. This inaccuracy could be reduced by using smaller discretization periods, but I did not want to do that because I wanted to decrease CPU load.

Then I decided to extend my controller with feed-forward that should calculate the base power that need to ensure rotation to the specified angle. In this case PID regulator should only compensate the impact of the external load.

PID Controller With Motion Profile Generator
Because Lego controller is focused on speed regulation, it cannot provide good precision in case when we need to reach a particular position and keep it. To provide such ability in my motor controller I decided to use  PID regulator that focused on position regulation. To provide speed regulation (especially with smooth acceleration) I implemented motion profile generator. It calculates the new position each regulation cycle. This approach allows me to implement different acceleration algorithms using different motion profile generators. I implemented two generators. Using them, the user can turn smooth acceleration on and off.

Example of generated motion profile with smooth acceleration

Example of generated motion profile without smooth acceleration

I tested version that used integer arithmetic and found that additional multiplications and divisions reduced performance. In fact, the motion profile generator that uses floating point works faster than the generator that uses integer arithmetic. I think this happens because the LeJOS executes Java byte-code using an interpreter.

Using Feed-forward to Improve Response to Command Signals
To use feed forward we should implement the system with two degrees of freedom.

The system with two degrees of freedom

On this diagram u_c is a position value that comes from the motion profile generator. H_c is our PID regulator it generates feed-back output, H_p is the motor, y is the tachometer value. H_m transfers the new position value to regulator. The most interesting part is H_u that generates feed-forward. This module should calculate the power that should be sent to the motor to ensure rotation to the specified position. In other words this module should implement inversion of transfer function of our motor.


Finding the inverse of the motor model
To find inverse of motor model we should find the motor model itself. I used following differential equations to describe mechanical and electrical state of DC motor:

where:

i e	(A) (V)	armature current armature voltage
ω τd	(rad/sec) (N·m)	shaft's angular velocity shaft's load torque
La Ra Kτ Kb	(H) (Ω) (N·m/A) (V/(rad/s))	armature inductance armature resistance torque constant back electromotive force coefficient
J B Ar	(kg·m²) (N·m/(rad/s)) (N·m)	rotor's moment of inertia viscosity resistance coefficient dry friction force

As I described in my previous post, I calculated these constants from measurements of motor under load performed by Philo and used them to emulate the NXT motor:
R_a = 5.262773292
K_b = 0.4952900056
K_τ = 0.3233728703
B = 0.0006001689451
L_a = 0.0047
J = 0.001321184025
A_r = 0.007299397206

To find the inversion of the transfer function we should make some approximations. I assumed:
ω(0) = ω₀, i(0) = f(ω₀).

To calculate f(ω0) I supposed that the system is in steady state (ω=const. and i=const.) at the initial moment. In this case these differential equations are represented by the following linear equations.

From this system of equations we can find i(0):

 Solving the equations we can find the angular velocity function:

We want to find the inverse function in form  voltage(distance, ω₀). To do this, we need to find integral of the angular velocity and then find the inverse function from it.

 The common solution is too complex to show here. I found a set of specific solutions for particular discretization periods. For example, for discretization periods 4ms and 25ms the inverse functions are:

power = (7299431.476·distance+11879.49780·signum(velocity)-28316.23421·velocity) / Battery.getVoltageMilliVolt()

power = (152012.7242·distance+11879.49771·signum(velocity)-2918.826420·velocity) / Battery.getVoltageMilliVolt()

To check accuracy of this function we can disable PID regulator and test the motor using only feed-forward component.

Feed-forward only regulation on emulator.

At the end of rotation the error is 1degree. This result has been got on emulator. We can expect that the controller will work without errors on emulator, but rounding the motor power value produces some non-linearity.
The following diagram shows the real motor measurement.

Feed-forward only regulation on Lego motor.

 We can see that position error is quite big (14 degrees at the end of rotation). Also we can see that the maximal motor's velocity is smaller than calculated velocity. I tuned K_b and A_r values and recalculated the feed-forward constants.

K_b=0.5002900056
A_r=0.009599397206

New feed-forward functions for 4ms and 25ms discretization periods are:

power = (7300460.329·distance + 15622.66220·signum(velocity) - 28311.62297·velocity) / Battery.getVoltageMilliVolt()

power = (152250.9950·distance + 15622.66225·signum(velocity) - 2916.056542·velocity) / Battery.getVoltageMilliVolt()

In this case the result looks better:

Feed-forward only regulation on Lego motor. Rotation to 400 degrees.

Feed-forward only regulation on Lego motor. Rotation to 4000 degrees.

The error on the first graph has maximum value 7.3 and is decreasing to 2 at the target point

Then we will test the controller with closed feed-back loop. I used two scenarios - rotation to 400 and 4000 degrees. To estimate impact of  feed-forward I will test the motor in two modes: feed-back only and feed-back with feed-forward. I used PI regulator in feed-back loop. The motor controller performs rotation to the specified angle and when this angle is reached and regulation error is less that 1 the controller switches to "stop" state where it keeps the current position. All following tests run until the motor switches to "stop" state.

Rotation to 400 degrees using only feed-back. Emulator.

Rotation to 4000 degrees using only feed-back. Emulator.

We can see that the motor does not reach the target position with the zero speed and performs some additional movements to reach it. Real motor shows the same behavior.

Rotation to 400 degrees using only feed-back. NXT motor.

Rotation to 4000 degrees using only feed-back. NXT motor.

We can see that results are very similar to results on emulator. Then we will test how feed-back and feed-forward are working together.

Rotation to 400 degrees using feed-back and feed-forward. Emulator.

Rotation to 4000 degrees using feed-back and feed-forward. Emulator.

We can see that combining feed-back and feed-forward reduces regulation error and the motor reaches the target point with zero speed.
Then we will test the controller using the real motor.

Rotation to 400 degrees using feed-back and feed-forward. NXT motor.

Rotation to 4000 degrees using feed-back and feed-forward. NXT motor.

We can see that real motor behavior is very similar to emulation. And the motor also reaches the target point with zero speed.

Feed-back regulator
As feed-back regulator I used PID regulator. I used algorithm described in [1]. I turned off derivative part and used the controller as PI controller, because derivative component produces high-frequency noise and makes rotation precision worse. But PI controller has some disadvantages.
Let's compare how LeJOS motor controller and my controller respond to change of load force. I will use the emulator to test scenario when the motor runs without load, but from 4 till 5 second some external load is applied to the motor's shaft.

Step response of my regulator

Step response of LeJOS regulator

We can see that LeJOS controller works more effective and its overshot is much lesser. But LeJOS uses 4ms discretization interval, it allows to increase proportional gain and derivative time and decrease integration time to improve response time of controller. I wanted to decrease CPU load, therefore I used discretization period 25ms. In this case I cannot use such parameter values because the controller becomes unstable.

CPU load
I used two tests to measure CPU load. In the first test I executed the regulation function in loop, on fake data, and measured average execution time. In the second test I tried to measure CPU load on working motor. This test increments an integer variable during 5 seconds. The first run is performed without running motors. Its results serves as a base. The second run is performed when one motor is running. Ration of these results gives me a percent of available CPU time.

Results of the first test:
My controller: 754 microseconds.
LeJOS controller: 595 microseconds.

Execution time of my controller is bigger because it uses multiple classes to implement its parts: motion profile generator, feed-forward calculator and PID regulator. When I combined all of them in a single class, I found that this variant of controller requires 597 microseconds to run.

The second test results (numbers of iterations):
My controlller: base: 374955, motor is running: 366222. CPU load 2.33%
LeJOS controller: base 374993, motor is running: 319285. CPU load 15%


These results are for one motor. When other motors are running we should multiple these values to number of working motors.

Conclusion
I wanted to make a motor controller that would provide the same interface as LeJOS motor but would consume less CPU time. I found that using long discretization periods together with feed-forward allow me to make such controller.
Another reason for starting of this project was the fact that LeJOS motor had not worked well with low acceleration values. However, the current version of LeJOS controller works well, but requires much CPU time.

By the way, I found another way to reduce CPU load in the motor controller. I converted my controller to C++, compiled it into native ARM code and tested it under nxtOSEK. In this case it requires only 61 microseconds to one regulation cycle. I think it is a interesting point to my next project.



Bibliography

[1] Åström, K. J. and T. Hägglund. Advanced PID control. - ISA - The Instrumentation, System, and
Automation Society, 2006