Reaction wheel inverted pendulum, cant get to steady state

Hi Guys,

I have rebuilt the reaction wheel inverted pendulum with some different parts.

Motor: https://shop.iflight-rc.com/ipower-motor-gm4108h-120t-gimbal-motor-with-as5048a-encoder-pro243?search=gm4108h

Encoder: [AS5048A Magnetic Encoder](https://AS5048A Magnetic Encoder)

I have adjusted the code from GitHub (GitHub - simplefoc/Arduino-FOC-reaction-wheel-inverted-pendulum: Reaction wheel inverted pendulum project based on the Arduino Simple FOC library and SimpleFOC shied.) so I can use the SPI connection the encoder offers. Right now I am trying to adjust the LQR controller and would appreciate some help.

#include <SimpleFOC.h>

// BLDC motor init
BLDCMotor motor = BLDCMotor(11);
// driver instance
BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8);

// Motor encoder init
MagneticSensorSPI encoder = MagneticSensorSPI(12, 14, 0x3FFF);
// pendulum encoder init
MagneticSensorSPI pendulum = MagneticSensorSPI(11, 14, 0x3FFF);
float pendulumoffset = 0;

void setup() {
  Serial.begin(115200);
  // initialise motor encoder hardware
  encoder.init();
  
  // init the pendulum encoder
  pendulum.init();
  pendulumoffset = pendulum.getAngle();
  
  // set control loop type to be used
  motor.controller = MotionControlType::torque;

  // link the motor to the encoder
  motor.linkSensor(&encoder);
  
  // driver
  driver.voltage_power_supply = 12; 
  driver.init();
  // link the driver and the motor
  motor.linkDriver(&driver);

   // limiting motor movements
  //motor.phase_resistance = 11.1; // [Ohm]
  //motor.current_limit = 1.5;   // [Amps] - if phase resistance defined

  // initialize motor
  motor.init();
  // align encoder and start FOC
  motor.initFOC();
}

// loop downsampling counter
long loop_count = 0;
float target_voltage;

void loop() {
  // ~1ms 
  motor.loopFOC();

  // control loop each ~25ms
  if(loop_count++ > 25){
    // updating the pendulum angle sensor
    // NECESSARY for library versions > v2.2 
    pendulum.update();
    
    // calculate the pendulum angle 
    float pendulum_angle = constrainAngle((pendulum.getAngle()-pendulumoffset) + M_PI);
    
    Serial.print(pendulum_angle);
    Serial.print("\n");
    
    if( abs(pendulum_angle) < 0.35 ) // if angle small enough stabilize
      target_voltage = controllerLQR(pendulum_angle, pendulum.getVelocity(), motor.shaftVelocity());
    else // else do swing-up
      // sets 45% of the maximal voltage to the motor in order to swing up
      target_voltage = -_sign(pendulum.getVelocity())*motor.voltage_limit*0.45;

    // set the target voltage to the motor
    motor.move(target_voltage);

    // restart the counter
    loop_count=0;
  }
   

}

// function constraining the angle in between -pi and pi, in degrees -180 and 180
float constrainAngle(float x){
    x = fmod(x + M_PI, _2PI);
    if (x < 0)
        x += _2PI;
    return x - M_PI;
}

// LQR stabilization controller functions
// calculating the voltage that needs to be set to the motor in order to stabilize the pendulum
float controllerLQR(float p_angle, float p_vel, float m_vel){
  // if angle controllable
  // calculate the control law 
  // LQR controller u = k*x
  //  - k = [40, 15, 0.35]
  //  - x = [pendulum angle, pendulum velocity, motor velocity]' 
  float u =  40*p_angle + 15*p_vel + 0.35*m_vel;
  
  // limit the voltage set to the motor
  if(abs(u) > motor.voltage_limit*0.7) u = _sign(u)*motor.voltage_limit*0.7;
  
  return u;
}

With the gains given in the example code the pendulum didn’t even came close to be controlled. I figured my motor could be heavier so played with the pendulum velocity and motor velocity. Right now I use the following gains:

k = [40, 15, 0.35]

x = [pendulum angle, pendulum velocity, motor velocity]

u = 40p_angle + 15p_vel + 0.35*m_vel

If I use this gains the pendulum almost stays in a steady state. I have recorded a video but it I cant upload it to this website. I have created a share link to download the video (Preformatted texthttps://we.tl/t-0wcZyYgu9f).

If I’m not mistaken the original reaction wheel is your project @Antun_Skuric?
Do you have any suggestions how I can get the pendulum to a steady state?

Thanks in advance.

Greetings from Tyrol.
Raphael

Sorry somehow the links got messed up

Motor: GM4108H-120T

Encoder: AS5048A Magnetic Encoder

Video

(Not talking from experience) Have you tried adding weights (balanced) near the circumference of the wheel.

Nice job btw. It looks like you are really close!

Hey @RaphaelS,

Does the pendulum stay upright when when you put it up without the swingup?
THis should be your first objective. Swing-up is just a cherry on the cake, once when your pendlum is capable of staying stable upright.

So I’d suggest you do to these steps:

  • set all the gains to the commander so you can set the quickly
  • remove the gains (set them to 0) for motor velocity and pendulum velocity, use only the gain for the pendulum ange (P controller)
  • you should reach almost stable behavior where the pendulum oscillates around 0 (upright) position
  • then try finding the gain for the pendulum velocity that makes your pendulum stay upright at the 0 position with almost no oscillations (but the motor velocity might drift and the motor might not stop)
  • then when you have those two more or less set (your pendulum is almost stable but your motor sometimes drifts to too high velocities and the pendulum falls) try setting the final gain for the motor velocity
  • this gain will make the motor try to stop whenever it can. So once you find a good value you should have a pendulum stable upright (small oscillations maybe) with the motor almost static.
  • in my experience the gains order or magnitude drops for each gaain:
    gain angle is around 10x gain for the pendulum velocity, and it is 10x the gain for the motor velocity.

From the video, I think you’re very close and I think you should enough actuation power to do everything necessary, its the gain finding time.

Good luck!

P.S. also make sure to test positive and negative gains for the motor velocity (it is sometimes inverted)

Hey @Owen_Williams

Thank you very much. I made two new designs of the wheel. One of the new designs has a thicker ring, the other new design has the thicker ring with mounts to fit M8 nuts so I can vary with the weights.

At the moment I am using the second wheel with 4 M8 nuts (can hold 8 nuts) and it is much more stable.

Thanks again.

Hey @Antun_Skuric

Thank you for the detailed explanation. I didn’t even think about the commander. My pendulum is now able to stay on top and even the swing up works most of the time. My pendulum is still oscillating but I think I found the reason. The pendulum encoder has a lot of noise interfering with the signal. In my opinion the noise is amplified by the mounting of the magnet used by the encoder. I will replace the steel mount with a plastic mount.

Thanks again.

2 Likes