2 axis gimbal stabilizer, shaking/precision issue

Hi everyone,

I’m new in this field, so first of all, thanks a lot for the authors and contributors of this library, it really helps me to start in BLDC driving.

I am making a 2 axis gimbal stabilizer, which will be used for precise pointing application. Here is my setup, divided in three parts with one MCU for each:

Motor control: One MCU (STM32L476) to control 2 motors. Drivers: 2x DRV8316, Motors 2x GM8112 with encoders (AS5048).

Inertial computing: The second (main) MCU (STM32L476) which achieve sensors fusion from the IMU and the magnetometer (LSM6DSO+LIS2MDL).

HMI: A third MCU (STM32F072) is used for handle joystick and screen (the HMI is deported from the rest of the system and communicate with the main MCU via RS485/UART)

(All electronics card are custom board)

The main MCU compute the error between actual angle (measured from sensors) and setpoint (adjustable with the joystick). The error is then sent to motor control MCU. I use simpleFOC in velocity mode as PID speed controller and the input is the error with a gain (P controller).

Here is a diagram of my actual system for one axis:

Note: as we can see on the diagram, I added a Kalman filter to my angle sensor measurement in order to reduce the noise from 0.15° to 0.008°, it help me to reduce a lot the motors vibrations and audible noise.

In term of timing, the angle error is sent at 100Hz by the main MCU via UART (921600 bauds) The Simplefoc specific part is call on interrupts at 4000Hz for both motors.

Initially I tuned the system parameters by evaluate different aspects:

  • The response of the motor to a small disturbance when speed =0
    I set speed =0 and manually apply a small disturbance to the motor, so I can adjust the parameters until getting a fast response and no bounce

  • The precision with which the motor maintains its setpoint angle.
    I add current estimation to improve the holding torque and the precision (by providing phase resistance and KV rating at init (BLDCMotor(21, 4.5f, 20) )
    When I set the speed setpoint to 0, and apply a small disturbance to the motor it remain to its original angle with an error range. The error is reduce from 0.34° to 0.1° with the current estimation.

  • The smoothness at slow speed
  • The audible noise of the motor or vibrations

The final step was to estimate the stabilization efficiency, for that I had a platform which can incline the gimbal in 3 axis. For the YAW axis I apply a rotation of ±10° with a 15s period. The best results I get are with the parameters below. The gimbal keeps the target angle with 1.5° range of error. The accuracy isn’t that good but the main problem is the little shaking, we can see on the picture below the little spikes at high frequency.

I get the same issue for the PITCH axis and the best I can have, is with those settings,

By doing some tests I see that the vibrations may vary, depending on the support on which the stabilizer is mounted… (I try to mount the gimbal on silent blocs and it’s even worse as it create some counter reaction between gimbal and the base)

So I would need some help to find a way to reduce the shaking and also to reduce the error range… If you have some ideas to better tune the parameters or any other suggestions to improve my actual system, I would really appreciate that :).

(For more setting details I join a simplified code for the motor control part)

//[…] Includes

// Private functions prototypes
void Update_IT_callback(void);

// Encoders
MagneticSensorSPI sensor_YAW = MagneticSensorSPI(CS_ENC_YAW, 14, 0x3FFF);
MagneticSensorSPI sensor_PITCH = MagneticSensorSPI(CS_ENC_PITCH, 14, 0x3FFF);

// BLDC Driver
// GM8112 motor: 36N42P → 21 pp ; 4.5 Ohm ; 400rpm/20V → 20KV
BLDCMotor motor_YAW = BLDCMotor(21, 4.5f, 20);
BLDCMotor motor_PITCH = BLDCMotor(21, 4.5f, 20);
// phA, phB, phC, CS
DRV8316Driver3PWM driver_YAW = DRV8316Driver3PWM(INHA_YAW, INHB_YAW, INHC_YAW, CS_DRV_YAW, false);
DRV8316Driver3PWM driver_PITCH = DRV8316Driver3PWM(INHA_PITCH, INHB_PITCH, INHC_PITCH, CS_DRV_PITCH, false);

// Seria Pins linkage
HardwareSerial Serial1(PB7, PB6); //RX, TX

// Flag and variables
float target_speed_YAW = 0;
float target_speed_PITCH = 0;
int read_OK =0;
int trame_OK =0;

void setup()
{
//Timer configuration
TIM_TypeDef *Instance = TIM3;
HardwareTimer *MyTim = new HardwareTimer(Instance);
MyTim->setOverflow(4000, HERTZ_FORMAT); // 4000 Hz
MyTim->attachInterrupt(Update_IT_callback);

// Init serial port
Serial1.begin(921600); // UART connected with main mcu

// Init magnetic encoders
_delay(100);
sensor_YAW.init();
sensor_PITCH.init();

// link the motor to the sensor
motor_YAW.linkSensor(&sensor_YAW);
motor_PITCH.linkSensor(&sensor_PITCH);

// driver config
driver_YAW.voltage_power_supply = MAX_SUPPLY_VOLTAGE;   // Volts
driver_YAW.voltage_limit = MAX_SUPPLY_VOLTAGE;			// Volts
driver_YAW.init();

driver_PITCH.pwm_frequency = 25000;
driver_PITCH.voltage_power_supply = MAX_SUPPLY_VOLTAGE;
driver_PITCH.voltage_limit = MAX_SUPPLY_VOLTAGE;
driver_PITCH.init();

// link the motor and the driver
motor_YAW.linkDriver(&driver_YAW);
motor_PITCH.linkDriver(&driver_PITCH);

// choose FOC modulation
motor_YAW.foc_modulation = FOCModulationType::SpaceVectorPWM;
motor_PITCH.foc_modulation = FOCModulationType::SpaceVectorPWM;

// set torque mode
motor_YAW.torque_controller = TorqueControlType::voltage;
motor_PITCH.torque_controller = TorqueControlType::voltage;

// set controler type
motor_YAW.controller = MotionControlType::velocity;
motor_PITCH.controller = MotionControlType::velocity;

// velocity PID controller parameters
motor_YAW.PID_velocity.P = 1.6f; 		 
motor_YAW.PID_velocity.I = 80.0f;		
motor_YAW.PID_velocity.D = 0.000f;		

motor_PITCH.PID_velocity.P = 1.2f;  	
motor_PITCH.PID_velocity.I = 60.0f;  	
motor_PITCH.PID_velocity.D = 0.000f;  

motor_PITCH.PID_velocity.output_ramp = 2000;
motor_YAW.PID_velocity.output_ramp = 2000;

motor_YAW.current_limit =4.0f; // Amps 
motor_PITCH.current_limit = 4.0f; // Amps

motor_YAW.voltage_limit = MAX_SUPPLY_VOLTAGE; // Volts
motor_YAW.PID_velocity.limit = motor_YAW.voltage_limit;

motor_PITCH.voltage_limit = MAX_SUPPLY_VOLTAGE; // Volts
motor_PITCH.PID_velocity.limit = motor_PITCH.voltage_limit;

// velocity low pass filtering time constant 
motor_YAW.LPF_velocity.Tf = 0.001f;
motor_PITCH.LPF_velocity.Tf = 0.001f;  


// maximal velocity of the position control
motor_YAW.velocity_limit = 0.4f;
motor_PITCH.velocity_limit = 0.4f;

// init motor hardware
motor_YAW.init();
motor_PITCH.init();

//Init YAW FOC
int init_FOC_state =0;
init_FOC_state  = motor_YAW.initFOC();

if(init_FOC_state !=1)
{
	HAL_NVIC_SystemReset(); //reset uC if init failed
}

//Init PITCH FOC
init_FOC_state = motor_PITCH.initFOC();
if(init_FOC_state !=1)  
{
	HAL_NVIC_SystemReset(); //reset uC if init failed
}


//Synchronisation sequences with external MCU via UART...
//[ ... ]
// 


//Start 4000Hz interrupts for SimpleFOC specific part
_delay(100);
MyTim->resume();

}

void loop()
{
trame_OK =0;

read_OK = receive_Serial(); //handle UART event: Check if datas are available on RX and read them (non blocking function)
if(read_OK == 1)
{
	trame_OK= extract_datas(&uart_datas);	//extract datas from UART frame
	if(trame_OK ==1 )
	{
		if(uart_datas.op == CMD_WRITE_SETPOINTS)   //check for command code, then set target speed
		{	
			//Set PITCH target speed
			if(abs(uart_datas.PITCH_error) < (0.01f* FROM_DEG_TO_RAD))  
			{
				target_speed_PITCH = 0.0f; 		//stop motor if 0.01° of precision is reach
			}
			else
			{
				//Apply (K) gain on the error 
				target_speed_PITCH = 17.0f*uart_datas.PITCH_error; 
			}
			  
			//Set YAW targer speed
			if(abs(uart_datas.YAW_error) < (0.01f* FROM_DEG_TO_RAD))	
			{	
				target_speed_YAW = 0.0f;	//stop motor if 0.01° of precision is reach
			}
			else
			{
				//Apply (K) gain on the error
				target_speed_YAW = 9.0f*uart_datas.YAW_error;
			}
			
		}

		//Check for motor physical angle limits
		if((sensor_PITCH.getMechanicalAngle() >= PITCH_MOT_MAX) && (target_speed_PITCH>0))
		{
			target_speed_PITCH = 0.0f;
		}
		else if((sensor_PITCH.getMechanicalAngle() <= PITCH_MOT_MIN) && (target_speed_PITCH<0))
		{
			target_speed_PITCH = 0.0f;
		}
	}
}

}

void Update_IT_callback(void) // 4 000Hz IT
{
motor_YAW.loopFOC();
motor_PITCH.loopFOC();

motor_PITCH.move(target_speed_PITCH);
motor_YAW.move(target_speed_YAW);

}

Hey @Vivien

This might be a velocity calculation issue. Could you plot your velocity curves?

Here’s a bit more about it: Stm32hwencoder - weird results - #9 by Antun_Skuric

1 Like

I second @Antun_Skuric about the speed computation. It seems you don’t use motor.motion_downsample, which in my experience is a key factor when using SimpleFOC. 4000 Hz sounds a bit too slow for motor.loopFOC() and a bit too fast for motor.move(). In my gimbal stabilizer (with smaller motors than yours), I use 10kHz for motor.loopFOC() and 1kHz for motor.move(), i.e. motor.motion_downsample = 10.

Not sure if it relates to your problem, but 100 Hz for your angle data rate sounds low. I’d say 400 Hz is a bare minimum, with 1kHz a more appropriate target.

I’m not sure if I read your graphics correctly, but if the yellow curves are the angles coming from your fusion algorithm, they look unusually noisy.

Hi Autun,

Thanks for the help

After some investigations, it appears that something is effectivly wrong with the speed:
(Here is the “shaftVelocity” during stabilisation)

Even when the motor is stopped it seem to be wrong, The speed oscillate around with a range of 0.02rad/s:

When I plot the angle I see that the oscillation has a range of 0.008° (0,00013962634 rad), even after the kalman filter… I don’t know if this lack of precision can cause the velocity issue


(green: unfiltered angle, purple: filtered angle)

After tunning more precisely the kalman filter, I also try to set the “sensor.min_elapsedtime” and it improves a little the results:


(Settings changed: sensor.min_elapsed_time = 0.002; + kalman filter parameters)

At this point I don’t know if the speed error is mostly due to an error of computation or if it came from my angle sensor. :thinking:

Hi Quentin

Thanks for your feedback !
I didn’t realize that my setup is a bit slow for the application…

I will try to use the downsample to enhance the performances.

May I ask if you use only one MCU for 2 two motors to reach 10kHz for motor.looFOC() or is it impossible for a single MCU? Because for now I get some difficulties to run it faster for 2 motors.

(For now I’m stuck to 100Hz for the datas fusion du to the limit of the library I use…)

I’m using a single MCU (ESP32-WROOM) for both motor control + fusion algorithm + stabilization computation (more complex than a standard gimbal system due to my specific application) + Bluetooth BLE (quite CPU intensive). I must add that I’m not using current sensing, which probably reduces CPU load.

I’m using GitHub - xioTechnologies/Fusion. I don’t use Kalman filtering at all.

1 Like