STM32实现水下四旋翼（十）飞行控制任务2——控制分配一. 控制分配的理论推导二. 控制分配的代码实现

53 阅读 0 评论 35 点赞

我是靠谱客的博主朴素篮球，最近开发中收集的这篇文章主要介绍STM32实现水下四旋翼（十）飞行控制任务2——控制分配一. 控制分配的理论推导二. 控制分配的代码实现，觉得挺不错的，现在分享给大家，希望可以做个参考。

概述

一. 控制分配的理论推导

众所周知四旋翼有四个电机，控制的最后一环就是控制四个电机的速度。上一节我们讲到姿态角串级PID控制，三个角度环输出了三个控制量，加上油门量，一共四个控制量，那么这四个量如何分别决定四个电机的速度呢？就需要设计一个控制分配器，如图所示：
在这里插入图片描述

这个图后面还会用到~

下图是我画的一般性四旋翼的布局图，可以看到是非对称的，因为我希望从一般情况下进行推导。其中四个电机标号从右上角顺时针旋转记为M1，M2，M3，M4。螺旋桨的旋向如图箭头所示。姿态传感器的安装如图中 $O{{x}_{b}}{{y}_{b}}{{z}_{b}}$ 所示。 $x$ 轴正方向指向后方， $y$ 轴正方向指向右侧， $z$ 轴正方向垂直向上（注意都是机体坐标系）。各轴的正方向按右手定则确定（拇指指向轴的正方向，四指方向为正），机体长度为 $2 L$ ，对角线夹角为 $2 α$ 。

在这里插入图片描述

单个螺旋桨的拉力可以表示为：
$T_{i}=c_{T} w_{i}^{2}$
其中 $c_{T}$ 为拉力系数，容易通过实验测量确定。反扭力大小表示为
$M_{i}=c_{M} w_{i}^{2}$
$M_{i}$ 表示螺旋桨叶 $i$ 在机身上产生的反扭力矩， $c_{M}$ 为反扭力矩系数，也可通过实验确定。

设螺旋桨对角线连线确定的安装夹角为 $2 α$ ，对角线螺旋桨间安装距离为 $2 L$ ，则滚转力矩和俯仰力矩的作用距离为 $L sin α$ 、 $L cos α$ 。

推进器作用在水下四旋翼上的总拉力为
$f=sum_{i=1}^{4} T_{i}=c_{T}left(w_{1}^{2}+w_{2}^{2}+w_{3}^{2}+w_{4}^{2}right)$

记 $tau_{x}, tau_{y} , tau_{z}$ 为螺旋桨产生的三轴力矩，可计算为
$tau_{x}=L sin alpha cdot c_{T}left(w_{1}^{2}+w_{2}^{2}-w_{3}^{2}-w_{4}^{2}right) \ tau_{y}=L cos alpha cdot c_{T}left(w_{1}^{2}-w_{2}^{2}-w_{3}^{2}+w_{4}^{2}right) \ tau_{z}=c_{M}left(w_{1}^{2}-w_{2}^{2}+w_{3}^{2}-w_{4}^{2}right) end{array}right.$

当 $={{45}^{o}}$ 时就是典型的X型布局，上式变为：
$tau_{x}=frac{sqrt{2}}{2} L cdot c_{T}left(w_{1}^{2}+w_{2}^{2}-w_{3}^{2}-w_{4}^{2}right) \ tau_{y}=frac{sqrt{2}}{2} L cdot c_{T}left(w_{1}^{2}-w_{2}^{2}-w_{3}^{2}+w_{4}^{2}right) \ tau_{z}=c_{M}left(w_{1}^{2}-w_{2}^{2}+w_{3}^{2}-w_{4}^{2}right) end{array}right.$

这个式子其实就说明了三轴的力矩在各个电机上的投影值。在确定电机的速度时按照这个值分配即可。

记四个推进器的速度分别为 $V_{M1}$ ， $V_{M2}$ ， $V_{M3}$ ， $V_{M4}$ ， $t_{V}$ 为遥控器摇杆输入的垂直方向推进的油门值。俯仰角PID输出为 $O_{text {pitch }}$ ，滚转角PID输出为 $O_{text {roll}}$ ，偏航角PID输出为 $O_{text {yaw}}$ 。三轴PID的输出就可以等效为输出力矩，那么按照上式的控制分配，将各个电机的速度写成下式：
$V_{M 1}=t_{V}+frac{sqrt{2}}{2} O_{r o l l}+frac{sqrt{2}}{2} O_{p i t c h}+O_{y a w} \ V_{M 2}=t_{V}+frac{sqrt{2}}{2} O_{r o l l}-frac{sqrt{2}}{2} O_{p i t c h}-O_{y a w} \ V_{M 3}=t_{V}-frac{sqrt{2}}{2} O_{r o l l}-frac{sqrt{2}}{2} O_{p i t c h}+O_{y a w} \ V_{M 4}=t_{V}-frac{sqrt{2}}{2} O_{r o l l}+frac{sqrt{2}}{2} O_{p i t c h}-O_{y a w} end{array}right.$

这就是我们要的控制分配率了。当然可以写成矩阵形式

$V = R ∙ I$

$R$ 就是控制矩阵
${{mathbf{V}}}={{[{{V}_{M1}},{{V}_{M2}},{{V}_{M3}},{{V}_{M4}}]}^{T}}$

${{mathbf{I}}}={{[{{t}_{V}},{{O}_{roll}},{{O}_{pitch}},{{O}_{yaw}}]}^{T}}$

$\ 1 & frac{sqrt{2}}{2} & -frac{sqrt{2}}{2} & -1 \ 1 & -frac{sqrt{2}}{2} & -frac{sqrt{2}}{2} & 1 \ 1 & -frac{sqrt{2}}{2} & frac{sqrt{2}}{2} & -1 \ end{matrix} right]$

用上面的方法，我们可以推导出十字型布局的四旋翼的控制分配，还有六旋翼、八旋翼的控制分配，以及不对称安装的多旋翼的控制分配。

二. 控制分配的代码实现

经过上面的推导，我们已经有了控制分配的算法了，在上一节姿态控制的基础上，我们继续往下走。上一节姿态角的串级PID计算已经得到了三个角度的控制量，从遥控器得到了油门值，直接应用上面推导出来的算法计算出电机的速度，就可以控制电机了。

新建一个pwm_control.c和pwm_control.h的文件。

pwm_control.h的内容为：

#ifndef __POWER_CONTROL_H
#define __POWER_CONTROL_H

#include "timer.h"
#include "stabilizer.h"

#define KV 500.0f           // 500 KV 电机
#define VOTAGE 25.2f
#define THRUST_MAX (KV*VOTAGE)
#define THRUST_MIN (-KV*VOTAGE)

void pwmControl(control_t *control);
void setMotorPWM(u32 m1_set, u32 m2_set, u32 m3_set, u32 m4_set);
u16 thrust2pwm(float thrust_value);
u16 thrust2pwmRange(float thrust_value, float p_min, float p_max);
float pwm2thrust(u16 pwm_value);
float pwm2Range(int pwm_value, float p_min, float p_max);
float limitThrust(float value);
u16 limitPWM(int input);

#endif

pwm_control.c的内容为：

#include "sys.h"
#include "pwm_control.h"
#include "timer.h"
#include "stabilizer.h"

float limitThrust(float value)
{
    if (value > THRUST_MAX)
    {
        value = THRUST_MAX;
    }
    else if (value < THRUST_MIN)
    {
        value = THRUST_MIN;
    }
    return value;
}

u16 limitPWM(int input)
{
    if (input > 2000)
        return 2000;
    else if (input < 1000)
        return 1000;
    else
        return input;
}

void pwmControl(control_t *control) /*功率输出控制*/
{
    float r = (float)control->roll * 0.707f;
    float p = (float)control->pitch * 0.707f;
    float y = (float)control->yaw;
    float thrust1, thrust2, thrust3, thrust4;

    thrust1 = limitThrust(control->thrust + r + p + y); //      |--------> Y
    thrust2 = limitThrust(control->thrust + r - p - y); //      |
    thrust3 = limitThrust(control->thrust - r - p + y); //      V
    thrust4 = limitThrust(control->thrust - r + p - y); //	     X

    motorPWM.m1 = thrust2pwm(thrust1);
    motorPWM.m2 = thrust2pwm(thrust2);
    motorPWM.m3 = thrust2pwm(thrust3);
    motorPWM.m4 = thrust2pwm(thrust4);
    // 将电机油门值转化为 pwm 值
    setMotorPWM(motorPWM.m1, motorPWM.m2, motorPWM.m3, motorPWM.m4);
}

void setMotorPWM(u32 m1_set, u32 m2_set, u32 m3_set, u32 m4_set)
{
    // 直接设定pwm值 
	TIM_SetTIM4Compare1(m1_set);
    TIM_SetTIM4Compare2(m2_set);
    TIM_SetTIM4Compare3(m3_set);
    TIM_SetTIM4Compare4(m4_set);
}

// 油门值转化为 PWM 值
u16 thrust2pwm(float thrust_value)
{
    float pwm_value;
    // 油门值转化为 PWM 1000-2000, [THRUST_MIN, THRUST_MAX] 到 [1000,2000]的映射
    pwm_value = 1000.0f + (thrust_value - THRUST_MIN) * 1000.0f / (THRUST_MAX - THRUST_MIN);
    return limitPWM((int)pwm_value);
}

float pwm2thrust(u16 pwm_value)
{
    float thrust_value;
    thrust_value = THRUST_MIN + ((float)pwm_value - 1000.0f) * (THRUST_MAX - THRUST_MIN) / 1000.0f;
    return limitThrust(thrust_value);
}

u16 thrust2pwmRange(float thrust_value, float p_min, float p_max)
{
    float pwm_value;
    // 油门值转化为 PWM pmin-pmax, [THRUST_MIN, THRUST_MAX] 到 [p_min,p_max]的映射
    pwm_value = p_min + (thrust_value - THRUST_MIN) * (p_max - p_min) / (THRUST_MAX - THRUST_MIN);
    return limitPWM((int)pwm_value);
}

float pwm2Range(int pwm_value, float p_min, float p_max)
{
    float p;
    p = p_min + ((float)(pwm_value)-1000.0f) * (p_max - p_min) / 1000.0f;
    if (p > p_max)
        p = p_max;
    if (p < p_min)
        p = p_min;
    return p;
}

定位到void pwmControl(control_t *control)，这里面就是按照前面的分配算法来确定四个电机的油门值，然后将油门值转化为对应的pwm值，setMotorPWM(motorPWM.m1, motorPWM.m2, motorPWM.m3, motorPWM.m4)直接就设定了四个电机的PWM值，也就是速度了，这样就实现了水下四旋翼的控制。

当然了，在上一节的基础上，我们只需要在姿态控制任务中增加一行代码就行了，就是pwmControl(&control)，修改后的姿态控制任务函数变为

void stabilization_task(void *p_arg)
{
	OS_ERR err;
	CPU_SR_ALLOC();
	
	u8 lenth = sizeof(configParam);
	lenth = lenth / 4 + (lenth % 4 ? 1 : 0);
	STMFLASH_Read(CONFIG_PARAM_ADDR, (u32 *)&configParam, lenth);   //载入PID参数

	attitudeControlInit();
	
	while(1)
	{
	    Water_Attitude_Control(&control);
	    pwmControl(&control);
		delay_ms(5);
	}
}

到了这一步，水下四旋翼已经可以在水里稳定的上浮下潜了（当然前提是PID参数调整对了），这时候拨动遥控器的俯仰和滚转摇杆，你会发现水下四旋翼跟着俯仰或滚转。然而的然而，它是不会像空中四旋翼那样前后左后飞行的，或者动的很慢。因为空中的阻力小，水中的阻力大，环境完全不同。所以水下四旋翼光靠机体的俯仰和滚转是很难实现前后左右机动的。推荐的方法是安装额外的水平的螺旋桨，提供水平分力。至于怎么设计那就是发挥你的聪明才智的时候啦！博主采用的方法不是水平电机，方案保密~~~增加水平电机在软件上的开销是很小的，只需要增加一路或几路PWM就行了。因为水下四旋翼已经提供了一个稳定的平台，现在只需要提供水平力就可以航行了。