Hello everyone,
Our team has been utilizing feedforward combined with proportional (P) control for our robotic arm this year, and it has been performing very well. However, as we aim to delve deeper into modern control theory for the FIRST Robotics Competition (FRC), we have developed a keen interest in the Linear Quadratic Regulator (LQR) controller.
While we have a general understanding of how LQR operates, we are currently encountering difficulties in modeling our motor, particularly in calculating parameters such as the moment of inertia (( J )). We are currently calculating the moment of inertia using ( J = \frac{1}{3} m L^2 ), but we are uncertain if this is the most accurate method for our system. Additionally, we are unsure how to calculate the friction coefficient ( b ) and the back EMF constant ( k_e ).
We attempted to create a Java class to simulate a NEO motor controlled by LQR. Here is our code:
package frc.robot.util;
public class NEOMotorLQR {
// State variables
private double theta; // Angular position (rad)
private double omega; // Angular velocity (rad/s)
private double current; // Motor current (A)
// Input voltage
private double voltage; // Input voltage (V)
// Load torque (can be modified as needed)
private double tauLoad = 0.0; // Load torque (Nm)
// Time step for updating motor state (seconds)
private double deltaTime = 0.02;
// LQR feedback gain matrix K
private final double[] K = { 2.5819889, -5.13244592, 0.22729785 };
// Target state variables
private double thetaRef = 0.0; // Target angular position (rad)
private double omegaRef = 0.0; // Target angular velocity (rad/s)
private double currentRef = 0.0; // Target motor current (A)
// Parameters of the NEO motor (need actual values)
private final double b = 0.0; // Friction coefficient (Nm·s/rad)
private final double k_t = 0.016; // Torque constant (Nm/A)
private final double k_e = 0.0; // Back EMF constant (V·s/rad)
private final double L = 0.000017; // Phase inductance (H)
private final double R = 0.033; // Phase resistance (Ω)
private final double J = 0.0; // Moment of inertia (kg·m^2)
/**
* Constructs the motor model with initial angular position, velocity, and current.
*
* @param theta Initial angular position (rad)
* @param omega Initial angular velocity (rad/s)
* @param current Initial motor current (A)
*/
public NEOMotorLQR(double theta, double omega, double current) {
this.theta = theta;
this.omega = omega;
this.current = current;
}
/**
* Updates the motor state using the given input voltage.
* Utilizes a state-space model for rigorous and accurate computation.
*
* @param inputVoltage Input voltage (V)
*/
public void updateState(double inputVoltage) {
// Set input voltage
this.voltage = inputVoltage;
// Calculate derivatives of the current state-space model
double dTheta1 = omega;
double dOmega1 = (k_t * current - b * omega - tauLoad) / J;
double dCurrent1 = (-R * current - k_e * omega + voltage) / L;
// Estimate intermediate states
double thetaMid = theta + dTheta1 * deltaTime / 2.0;
double omegaMid = omega + dOmega1 * deltaTime / 2.0;
double currentMid = current + dCurrent1 * deltaTime / 2.0;
// Calculate k2 using estimated intermediate states
double dTheta2 = omegaMid;
double dOmega2 = (k_t * currentMid - b * omegaMid - tauLoad) / J;
double dCurrent2 = (-R * currentMid - k_e * omegaMid + voltage) / L;
// Update state variables
theta += dTheta2 * deltaTime;
omega += dOmega2 * deltaTime;
current += dCurrent2 * deltaTime;
}
/**
* Computes the control input voltage based on the LQR control law.
*
* @return Control input voltage (V)
*/
public double computeControlInput() {
// State vector [theta, omega, current]
double[] x = { theta, omega, current };
// Reference state vector [thetaRef, omegaRef, currentRef]
double[] xRef = { thetaRef, omegaRef, currentRef };
double controlInput = 0.0;
// Compute state error xError = x - xRef
double[] xError = new double[x.length];
for (int i = 0; i < x.length; i++) {
xError[i] = x[i] - xRef[i];
}
// Compute control input u = -K * xError
for (int i = 0; i < K.length; i++) {
controlInput -= K[i] * xError[i];
}
return controlInput;
}
/**
* Updates the motor state and applies LQR control.
*/
public void updateMotor() {
double inputVoltage = computeControlInput();
updateState(inputVoltage);
}
// Getter and Setter methods
public double getTheta() {
return theta;
}
public double getOmega() {
return omega;
}
public double getCurrent() {
return current;
}
public void setLoadTorque(double tauLoad) {
this.tauLoad = tauLoad;
}
public void setDeltaTime(double deltaTime) {
this.deltaTime = deltaTime;
}
/**
* Sets the target angular position (rad)
*
* @param thetaRef Target angular position (rad)
*/
public void setThetaRef(double thetaRef) {
this.thetaRef = thetaRef;
}
/**
* Sets the target angular velocity (rad/s)
*
* @param omegaRef Target angular velocity (rad/s)
*/
public void setOmegaRef(double omegaRef) {
this.omegaRef = omegaRef;
}
/**
* Sets the target motor current (A)
*
* @param currentRef Target motor current (A)
*/
public void setCurrentRef(double currentRef) {
this.currentRef = currentRef;
}
}
We are facing challenges in the following areas:
-
Calculation of the Moment of Inertia (( J )): We are currently calculating the moment of inertia using ( J = \frac{1}{3} m L^2 ), but we are uncertain if this is the most accurate method for our system.
-
Determining Motor Parameters: We are unsure how to calculate the friction coefficient ( b ) and the back EMF constant ( k_e ).
-
Designing the LQR Gain Matrix ( K ): We have used a set of preset gain values but need to understand how to correctly compute these gains based on our system to ensure optimal controller performance.
If anyone has similar experience and can share insights or suggestions on motor modeling and controller design, we would greatly appreciate it.
Thank you!
.