Tabla de contenido
1. Principio de control de profundidad
Ya hemos hablado antes del control PID en cascada del ángulo de actitud, por lo que ahora es muy fácil entender el control de profundidad. Todos son controles de posición, aquí solo uso un controlador PID de una etapa para el control de profundidad, que es mucho más simple. La profundidad deseada proviene de la integral de la palanca de control remoto (el valor de velocidad del movimiento hacia arriba y hacia abajo del canal del acelerador en el modo de profundidad fija), y la profundidad de retroalimentación proviene de los datos del sensor de profundidad del agua. El principio de control es:
Esta figura apareció en el capítulo anterior. Aquí agregamos un bucle de posición para el control de profundidad, pero no agregué un bucle de velocidad para el eje Z. Los experimentos han demostrado que la profundidad se puede determinar básicamente. Por supuesto, la respuesta no es muy rápida, mejore el algoritmo.
Es necesario establecer un valor de referencia del acelerador de profundidad fijo durante el control de profundidad fijo. Bajo este valor, la flotabilidad del quadrotor se equilibra básicamente con la gravedad, de modo que básicamente puede flotar. La salida del anillo de profundidad se superpone a este valor básico del acelerador para que se mueva hacia arriba y hacia abajo. Este valor de referencia se puede ajustar artificialmente, al igual que los parámetros PID, también se puede calcular mediante algoritmos. El método consiste en detectar la diferencia entre el valor del acelerador y el valor de referencia del acelerador establecido en tiempo real cada vez que se activa el control de profundidad. Después de un período de tiempo, el quadcopter flotará aproximadamente (incluso si el valor de referencia del acelerador establecido tiene una gran desviación), si se detecta la diferencia Si es muy grande, significa que el valor base del acelerador establecido actualmente es incorrecto, por lo que puede establecer el valor actual real del acelerador como el nuevo valor base del acelerador. La ventaja de esto es que una vez que el peso del quadcopter subacuático cambia debido a la carga o reemplazo de piezas, no hay necesidad de pensar en el ajuste.
2. Agregue la función de control de profundidad para tareas de control de actitud
Cree un archivo position_pid.hy position_pid.c para solucionar el problema del control de posición. No me disguste por crear tantos archivos. Por favor, desarrolle este hábito. Poner diferentes funciones en diferentes archivos hará que el código sea muy legible.
archivo position_pid.h:
#ifndef __POSITION_PID_H
#define __POSITION_PID_H
#include "sys.h"
#include "stabilizer.h"
#include "pid.h"
#define POS_UPDATE_RATE 200
#define POS_UPDATE_DT (1.0f / POS_UPDATE_RATE)
#define PID_DEPTH_INTEGRATION_LIMIT 10000.0
typedef struct
{
PidObject pidVX;
PidObject pidVY;
PidObject pidVZ;
float thrustBase; // 定深时的油门基准值，这个值可以让四轴悬停
bool preMode; // 前一次的模式，为true时对应定深模式
bool isAltHoldMode; // 为true时对应定深模式
}posPid_t;
void positionControlInit(void);
void positionResetAllPID(void);
void depthPID(float *actualDepth, float *desiredDepth, control_t *output);
void getPositionPIDZ(float* kp, float* ki, float* kd);
void setPositionPIDZ(float kp, float ki, float kd);
extern posPid_t posPid;
#endif
archivo position_pid.c:
#include "position_pid.h"
#include "stabilizer.h"
#include <math.h>
#include "pid.h"
#include "pwm_control.h"
//#define THRUST_SCALE (5.0f)
#define THRUST_SCALE (1.0f)
#define START_DEPTH (0.0f)
#define THRUSTBASE_HIGH (10000.f)
#define THRUSTBASE_LOW (-10000.f)
posPid_t posPid;
/*基础油门值限制*/
float limitThrustBase(float input)
{
if(input > THRUSTBASE_HIGH)
return THRUSTBASE_HIGH;
else if(input < THRUSTBASE_LOW)
return THRUSTBASE_LOW;
else
return input;
}
void positionControlInit(void)
{
pidInit(&posPid.pidVZ, 0, configParam.pidPos.vz, POS_UPDATE_DT); /*vz PID初始化*/
pidSetIntegralLimit(&posPid.pidVZ, PID_DEPTH_INTEGRATION_LIMIT); /*roll 角度积分限幅设置*/
pidSetOutLimit(&posPid.pidVZ, PID_DEPTH_INTEGRATION_LIMIT);
positionResetAllPID();
posPid.thrustBase = limitThrustBase(configParam.thrustBase); // 每次初始化重新给定油门基值
}
深度环PID
void depthPID(float *actualDepth, float *desiredDepth, control_t *output)
{
float PIDoutThrust;
float depthError = *desiredDepth - *actualDepth;
PIDoutThrust = THRUST_SCALE * pidUpdate(&posPid.pidVZ,depthError);
if (posPid.isAltHoldMode == true) // 在定高模式下检测机体的重量，自动调整定高油门基准值
{
//detectWeight(PIDoutThrust); // 检测重量，更新油门值
}
// thrustBase是负值
output->thrust = limitThrust(posPid.thrustBase - PIDoutThrust); // z轴向下为正，油门值向上为正
}
void positionResetAllPID(void)
{
//pidReset(&posPid.pidVX);
//pidReset(&posPid.pidVY);
pidReset(&posPid.pidVZ);
}
void getPositionPIDZ(float* kp, float* ki, float* kd)
{
*kp = posPid.pidVZ.kp;
*ki = posPid.pidVZ.ki;
*kd = posPid.pidVZ.kd ;
}
void setPositionPIDZ(float kp, float ki, float kd)
{
posPid.pidVZ.kp = kp;
posPid.pidVZ.ki = ki;
posPid.pidVZ.kd = kd;
}
void depthPID(float *actualDepth, float *desiredDepth, control_t *output)Se realiza el cálculo del bucle de profundidad y el resultado del cálculo se superpone al valor de referencia del acelerador como salida. Uno de ellos detectWeight(PIDoutThrust)es detectar automáticamente el peso y actualizar el valor del acelerador. Lo comenté aquí porque no tengo tiempo para depurar esta función más adelante en el proyecto. Aunque se ha escrito, no ha pasado la prueba de tiempo, y no pondré las cosas que no se han probado.
Bien, aquí nos hemos dado cuenta de la función de navegación de profundidad fija. Ahora necesitamos actualizar la void Water_Attitude_Control(control_t *output)función. Antes, solo escribíamos la función de modo manual en ella. Ahora agregue el modo de profundidad fija.
void Water_Attitude_Control(control_t *output)
{
float turn_speed;
float z_speed;
float forward_speed;
/*************************** 针对 手动模式和定高模式 以及 模式切换做预处理 ***********************************/
// 手动模式下油门通道转化为油门基准值
if (command[CARRY_MODE] == HAND_MODE)
{
posPid.isAltHoldMode = false;
}
// 定高（定深）时油门保持在设定的基准值，这个基准值刚好让机器人悬浮，油门通道此时转化为 z 轴速度
else if (command[CARRY_MODE] == DEPTH_MODE)
{
posPid.isAltHoldMode = true;
}
// 由手动模式切换到定深模式 // 由定深模式切换到手动模式
if ((posPid.isAltHoldMode == true && posPid.preMode == false) ||
(posPid.isAltHoldMode == false && posPid.preMode == true))
{
positionResetAllPID();
control.depthOut = 0; // 深度环PID置0
}
posPid.preMode = posPid.isAltHoldMode; // 当前模式变为pre模式
turn_speed = pwm2Range(command[YAW], -1000.0f, 1000.0f);
if (turn_speed < 30.0f && turn_speed > -30.0f)
turn_speed = 0.f; // 死区
z_speed = pwm2Range(command[THROTTLE], -1000.0f, 1000.0f);
if (z_speed < 30.0f && z_speed > -30.0f)
z_speed = 0.f; // 死区
/*************************** 针对 手动模式和定高模式 以及 模式切换做预处理 ***********************************/
/*************************************** 定高模式下控制 ************************************************/
// 高度环 PID 计算并作用到油门值，调整直到达到悬浮状态
if (posPid.isAltHoldMode == true)
{
// 不使能时，电机锁定
if (command[ALTHOLD_ENABLE] == HOLD_DISABLE) // 定高禁止，不运动，复位所有PID
{
attitudeResetAllPID(); //PID复位
positionResetAllPID();
setstate.expectedAngle.yaw = state.realAngle.yaw;
setstate.expectedAngle.roll = state.realAngle.roll;
setstate.expectedAngle.pitch = state.realAngle.pitch;
setstate.expectedDepth = state.realDepth;
control.thrust = 0;
control.yaw = 0;
control.roll = 0;
control.pitch = 0;
control.depthOut = 0; // 深度环PID置0
}
// 使能时 开始定高控制
else if (command[ALTHOLD_ENABLE] == HOLD_ENABLE)
{
setstate.expectedAngle.roll = pwm2Range(command[ROLL], -30.0f, 30.0f);
if (setstate.expectedAngle.roll < 0.9f && setstate.expectedAngle.roll > -0.9f)
setstate.expectedAngle.roll = 0.f; // 摇杆死区
setstate.expectedAngle.pitch = pwm2Range(command[PITCH], -30.0f, 30.0f);
if (setstate.expectedAngle.pitch < 0.9f && setstate.expectedAngle.pitch > -0.9f)
setstate.expectedAngle.pitch = 0.f; //遥感死区
setstate.expectedAngle.yaw -= turn_speed * zoom_factor_yaw * ft;
if (setstate.expectedAngle.yaw > 180.0f)
setstate.expectedAngle.yaw -= 360.0f;
if (setstate.expectedAngle.yaw < -180.0f)
setstate.expectedAngle.yaw += 360.0f;
setstate.expectedDepth -= z_speed * zoom_factor_vz * ft; // 向上调整时期望深度减小
depthPID(&state.realDepth, &setstate.expectedDepth, &control);
attitudeAnglePID(&state.realAngle, &setstate.expectedAngle, &setstate.expectedRate); /* 角度环PID */
attitudeRatePID(&state.realRate, &setstate.expectedRate, &control); /* 角速度环PID */
}
}
/*************************************** 定高模式下控制 ************************************************/
/******************************************* 手动模式下控制 ********************************************/
if (posPid.isAltHoldMode == false)
{
// 手动模式下 油门通道为0时不运动，复位所有 姿态pid与pid输出
control.thrust = pwm2thrust(command[THROTTLE]);
setstate.expectedDepth = state.realDepth; // 手动模式下期望高度始终等于当前高度/
// 切换到定高时从当前高度开始定高
if (control.thrust < 200 && control.thrust > -200)
control.thrust = 0; // 油门死区
if ((int)control.thrust == 0 && (int)turn_speed == 0) // 油门和方向摇杆都居中，机器人不使能
{
attitudeResetAllPID(); //PID复位
setstate.expectedAngle.yaw = state.realAngle.yaw;
setstate.expectedAngle.roll = state.realAngle.roll;
setstate.expectedAngle.pitch = state.realAngle.pitch;
control.yaw = 0;
control.roll = 0;
control.pitch = 0;
}
else // 油门有输出，从遥控器获得期望值，姿态PID // 或者油门没输出，原地转圈
{
setstate.expectedAngle.roll = pwm2Range(command[ROLL], -30.0f, 30.0f);
if (setstate.expectedAngle.roll < 0.9f && setstate.expectedAngle.roll > -0.9f)
setstate.expectedAngle.roll = 0.f; // 摇杆死区
setstate.expectedAngle.pitch = pwm2Range(command[PITCH], -30.0f, 30.0f);
if (setstate.expectedAngle.pitch < 0.9f && setstate.expectedAngle.pitch > -0.9f)
setstate.expectedAngle.pitch = 0.f; //遥感死区
setstate.expectedAngle.yaw -= turn_speed * zoom_factor_yaw * ft;
if (setstate.expectedAngle.yaw > 180.0f)
setstate.expectedAngle.yaw -= 360.0f;
if (setstate.expectedAngle.yaw < -180.0f)
setstate.expectedAngle.yaw += 360.0f;
attitudeAnglePID(&state.realAngle, &setstate.expectedAngle, &setstate.expectedRate); /* 角度环PID */
attitudeRatePID(&state.realRate, &setstate.expectedRate, &control); /* 角速度环PID */
}
}
/******************************************* 手动模式下控制 ********************************************/
}
Bien, ahora se ha agregado el modo de control de profundidad, y el modo se puede cambiar a través de un interruptor de dial en el control remoto. No es necesario cambiar la función principal, porque se llama en la tarea de control de actitud void Water_Attitude_Control(control_t *output). Este es el beneficio de la encapsulación.
En este punto, la tarea de control ha terminado, la próxima lección dará inicio al desarrollo de la computadora superior, por supuesto, el desarrollo de la computadora superior requiere que la computadora inferior formule el mismo protocolo de comunicación, y continuaremos más adelante.