一. 定深控制原理
前面我们讲过了姿态角的串级PID控制，那么现在来理解定深控制是非常好理解了。他们都是位置控制，这里的深度控制我只用了单级的PID控制器，显得会更简单。期望深度来源于遥控器摇杆的积分（在定深模式下油门通道变为上下运动的速度值），反馈深度来自于水深传感器的数据。控制原理为：
这个图上一章就出现了，这里我们增加深度控制的位置环，只不过我没有加入Z轴的速度环，实验证明基本能实现定深，当然了响应不是很快，后续可以改进算法。
定深控制时需要设置一个定深油门的基准值，在这个值下，四旋翼浮力与重力基本平衡，从而基本能够悬浮，深度环的输出叠加在这个油门基础值上，就可以使其上下机动。这个基准值可以人为去调参，如同PID参数一样，也可以通过算法去计算。方法就是每次启动定深控制后实时检测油门值与设定的油门基准值的差值，一段时间后四旋翼会大致悬浮（即使设定的油门基准值偏差很大），如果检测到差值很大，说明当前设定的油门基值不对，就可以将当前的实际油门值设定为新的油门基准值。这样的好处就是一旦水下四旋翼由于负载或更换部件导致重量变了，就不需要认为去调参了。
二. 姿态控制任务增加定深控制功能
建立一个position_pid.h和position_pid.c文件，专门处理位置控制的问题。大家不要嫌弃我建了这么多文件哈，请养成这个习惯，将不同的功能放在不同的文件里，代码的可读性会非常强。
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
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)实现了深度环的计算，计算结果叠加到油门基准值，作为输出。里面有一个detectWeight(PIDoutThrust)就是实现自动检测重量并更新油门值的哈，这里我注释掉了，因为项目后期我没有时间调试这个功能了。虽然已经写了，但是没有通过时间检验，没有经过检验的东西我不放上来。
好的，到这里我们实现了定深航行的功能了，现在我们需要更新void Water_Attitude_Control(control_t *output)函数，之前我们只在里面写了手动模式的功能，现在加入定深模式。
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 */
}
}
/******************************************* 手动模式下控制 ********************************************/
}
好的，现在加入了定深控制的模式，可以通过遥控器的一个拨码开关进行模式切换。在main函数中不需要进行更改了，因为姿态控制任务里面是调用的void Water_Attitude_Control(control_t *output)。这就是封装的好处啦。
到这里控制任务就结束了，下一讲开始进行上位机的开发，当然了上位机开发需要下位机制定相同的通信协议，后面继续。
1万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
