1.深さ制御の原理
姿勢角のカスケードPID制御については前に説明したので、深度制御が非常に理解しやすくなりました。これらはすべて位置制御です。ここでは、深度制御に1段階のPIDコントローラーのみを使用します。これははるかに簡単です。必要な深さは、リモートコントロールスティックの積分（固定深さモードでのスロットルチャネルの上下運動の速度値）から得られ、フィードバックの深さは、水深センサーのデータから得られます。制御原理は次のとおりです。
この図は前の章で説明しました。ここでは深度制御用の位置ループを追加しましたが、Z軸の速度ループは追加しませんでした。実験により、深度は基本的に決定できることが証明されました。もちろん、応答はあまり速くありません。アルゴリズムを改善してください。
固定深度制御時に固定深度スロットルの基準値を設定する必要があります。この値では、クワッドローターの浮力は基本的に重力と釣り合っているため、基本的に浮くことができます。これに深度リングの出力が重ね合わされます。スロットルを上下に動かすための基本値。この参照値は、PIDパラメーターと同様に人為的に調整でき、アルゴリズムによって計算することもできます。この方法は、深度制御がアクティブになるたびに、スロットル値と設定されたスロットル基準値の差をリアルタイムで検出する方法です。一定時間後、クアッドコプターは大まかに浮きます（設定されたスロットル基準値が大きい場合でも）偏差）、差が検出された場合差が非常に大きい場合は、現在設定されているスロットルベース値が間違っていることを意味するため、現在の実際のスロットル値を新しいスロットルベース値として設定できます。これの利点は、負荷や部品の交換によって水中クワッドコプターの重量が変化すると、チューニングについて考える必要がないことです。
2.姿勢制御タスク用の深度制御機能を追加します
位置制御の問題に対処するために、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)深度ループの計算が実現され、計算結果が出力としてスロットル基準値に重ね合わされます。そのうちの1つは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)。これがカプセル化の利点です。
この時点で制御タスクは終了し、次の講義で上位コンピュータの開発を開始します。もちろん、上位コンピュータの開発では下位コンピュータが同じ通信プロトコルを作成する必要があり、後で続行します。