Controlador vertical
Nesta secção você irá implementar o controlador vertical, que comanda a força de empuxo \({\color{var(--c2)}f_t}\) a partir da diferença entre a posição vertical de referência \({\color{var(--c3)}z_r}\) e estimada \({\color{var(--c1)}z}\).
Para isto, será implementada uma nova função:
verticalController()
Além de uma alteração em uma função já previamente implementada:
reference()
Implementação
Para começar, copie e cole o arquivo vertical_estimator.c e renomeie ele para vertical_controller.c.
Definições
Variáveis globais
Declare mais uma variável global, que corresponde à posição vertical de referência \({\color{var(--c3)}z_r}\).
// Actuators
float pwm1, pwm2, pwm3, pwm4; // Motors PWM
// Sensors
float ax, ay, az; // Accelerometer [m/s^2]
float gx, gy, gz; // Gyroscope [rad/s]
float d; // Range [m]
// System inputs
float ft; // Thrust force [N]
float tx, ty, tz; // Roll, pitch and yaw torques [N.m]
// System states
float phi, theta, psi; // Euler angles [rad]
float wx, wy, wz; // Angular velocities [rad/s]
float z; // Vertical position [m]
float vz; // Vertical velocity [m/s]
// System references
float phi_r, theta_r, psi_r; // Euler angles reference [rad]
float z_r // Vertical position reference [m]
Loop principal
Inclua a chamada da função verticalController() no loop principal.
// Main application task
void appMain(void *param)
{
// Infinite loop (runs at 200Hz)
while (true)
{
reference(); // Read reference setpoints (from Crazyflie Client)
sensors(); // Read raw sensor measurements
attitudeEstimator(); // Estimate orientation (roll/pitch/yaw) from IMU sensor
verticalEstimator(); // Estimate vertical position/velocity from range sensor
verticalController(); // Compute desired thrust force
attitudeController(); // Compute desired roll/pitch/yaw torques
mixer(); // Convert desired force/torques into motor PWM
actuators(); // Send commands to motors
vTaskDelay(pdMS_TO_TICKS(5)); // Loop delay (5 ms)
}
}
Funções
Referência
A posição vertical de referência \({\color{var(--c3)}z_r}\) será comandada pelo Command Based Flight Control do Crazyflie Client utilizando os botões Up e Down.
Modifique a função reference() para que a posição vertical de referência \({\color{var(--c3)}z_r}\) seja definida pela variável setpoint.position.z.
// Get reference setpoints from commander module
void reference()
{
// Declare variables that store the most recent setpoint and state from commander
static setpoint_t setpoint;
static state_t state;
// Retrieve the current commanded setpoints and state from commander module
commanderGetSetpoint(&setpoint, &state);
// Extract position references from the received setpoint
z_r = setpoint.position.z; // Z position reference [m]
phi_r = (setpoint.position.y * 2.0f) * pi/4.0f; // Roll reference command [rad] (maps 0.5m -> pi/4 rad)
theta_r = (setpoint.position.y * 2.0f) * pi/4.0f; // Pitch reference command [rad] (maps 0.5m -> pi/4 rad)
psi_r = 0.0f; // Yaw reference command [rad]
}
Controlador vertical
A função verticalController() é quem comanda a força de empuxo \({\color{var(--c2)}f_t}\) a partir da diferença entre a posição vertical de referência \({\color{var(--c3)}z_r}\) e estimada \({\color{var(--c1)}z}\).
// Compute desired thrust force
void verticalController()
{
}
Já vimos que a dinâmica linearizada de um quadricóptero pode ser representada pelo diagrama de blocos abaixo:
A dinâmica de posição vertical é descrita pelo seguinte trecho:
Podemos cancelar a massa e a aceleração da gravidade de modo que a variável de controle seja a aceleração vertical:
Isso reduz o sistema a ser controlado a um integrador duplo, exatamente como fizemos com o controlador de atitude. No entanto, agora temos um problema adicional: como estamos somando o termo da aceleração da gravidade (diferentemente da massa que está sendo multiplicada), caso ele seja um pouco diferente do real, não acontecerá um cancelamento exato e o sistema possuirá erro em regime permanente. Para resolver esse problema, podemos incluir um integrador no controlador.
O controlador proporcional-integral-derivativo (PID) adiciona ao controlador proporcional-derivativo (PD) um termo integral que acumula o erro ao longo do tempo, eliminando o erro estacionário. A ação proporcional e derivativa garantem resposta rápida e amortecida, enquanto a integral corrige desvios persistentes. É versátil e eficaz para o integrador duplo, mas o termo integral exige cuidado para evitar oscilações de baixa frequência (windup) e lentidão na resposta.
Definição dos ganhos \(k_p\), \(k_i\) e \(k_d\)
Olhando o controlador isoladamente, temos o seguinte diagrama de blocos:
Que se traduz nas equações abaixo:
Inclua na função attitudeController() três variáveis locais \(k_p\), \(k_i\) e \(k_d\), que correspondem aos ganhos do controlador, e, em seguida, calcule o torque comandado \({\color{var(--c2)}f_t}\) seguindo as equações acima(1).
-
A integral do erro \(\displaystyle\int z_e dt\) pode ser calculada com um termo auxiliar utilizando a seguinte lógica:
// Compute integral term (static to retain value amoung function calls) static float z_e_integral; z_e_integral += z_e*dt;A derivada do erro \(\dfrac{dz_e}{dt}\) pode ser calculada com um termo auxiliar utilizando a seguinte lógica:
// Compute derivative term with auxiliary variable for previous error (static to retain value amoung function calls) static float z_e_previous; float z_e_derivative = (z_e-z_e_previous)/dt; z_e_previous = z_e;
// Compute desired thrust force
void verticalController()
{
// Controller parameters (settling time of 2.0s and overshoot of 0,05%)
static const float kp =
static const float ki =
static const float kd =
// Compute vertical position error
float z_e =
// Compute desired thrust force
ft =
}
O controlador proporcional em cascata com ação integral (PI-P) combina uma malha interna proporcional com uma malha externa proporcional com ação integral. A malha interna garante resposta rápida e amortecida, enquanto o termo integral na malha externa elimina o erro estacionário de posição. Essa configuração equilibra desempenho e simplicidade, oferecendo boa robustez sem exigir integrações redundantes, mas requer sintonia coordenada entre as duas malhas.
Definição dos ganhos \(k_p\), \(k_i\) e \(k_d\)
Olhando o controlador isoladamente, temos o seguinte diagrama de blocos:
Que se traduz nas equações abaixo:
Inclua na função attitudeController() três variáveis locais \(k_p\), \(k_i\) e \(k_d\), que correspondem aos ganhos do controlador, e, em seguida, calcule o torque comandado \({\color{var(--c2)}f_t}\) seguindo as equações acima(1).
-
A integral do erro \(\displaystyle\int z_e dt\) pode ser calculada com um termo auxiliar utilizando a seguinte lógica:
// Compute integral term (static to retain value amoung function calls) static float z_e_integral; z_e_integral += z_e*dt;
// Compute desired thrust force
void verticalController()
{
// Controller parameters (settling time of 2.0s and overshoot of 0,05%)
static const float kp =
static const float ki =
static const float kd =
// Compute vertical position error
float z_e =
// Compute desired thrust force
ft =
}
O regulador quadrático com ação integral (LQI) estende o regulador quadrático linear (LQR) adicionando uma variável que integra o erro de saída. Isso permite eliminar o erro estacionário sem perder as vantagens do controle por realimentação completa. A estrutura resultante combina desempenho dinâmico ajustável — por meio do posicionamento dos polos — com precisão em regime permanente. É uma solução elegante e sistemática, mas requer modelagem ampliada e cálculo de ganhos por métodos de espaço de estados.
Definição dos ganhos \(k_p\), \(k_i\) e \(k_d\)
Olhando o controlador isoladamente, temos o seguinte diagrama de blocos(1):
- No sistema linearizado temos que \({\color{var(--c3)}\dot{z}_r} = {\color{var(--c3)}v_{z_r}}\) e \({\color{var(--c1)}\dot{z}} = {\color{var(--c1)}v_z}\).
Que se traduz na equação abaixo(1):
-
Como o objetivo é deixar o quadricóptero estacionário, a velocidade angular de referência \({\color{var(--c3)}v_{z_r}}\) pode ser assumida como sendo zero, o que reduz um dos termos:
\[ k_d \left( \cancelto{0}{{\color{var(--c3)}v_{z_r}}} - {\color{var(--c1)}v_z} \right) = - k_d {\color{var(--c1)}v_z} \]
Inclua na função attitudeController() três variáveis locais \(k_p\), \(k_i\) e \(k_d\), que correspondem aos ganhos do controlador, e, em seguida, calcule o torque comandado \({\color{var(--c2)}f_t}\) seguindo as equações acima(1).
-
A integral do erro \(\displaystyle\int z_e dt\) pode ser calculada com um termo auxiliar utilizando a seguinte lógica:
// Compute integral term (static to retain value amoung function calls) static float z_e_integral; z_e_integral += z_e*dt;
// Compute desired thrust force
void verticalController()
{
// Controller parameters (settling time of 2.0s and overshoot of 0,05%)
static const float kp =
static const float ki =
static const float kd =
// Compute vertical position error
float z_e =
// Compute desired thrust force
ft =
}