在线观看www成人影院-在线观看www日本免费网站-在线观看www视频-在线观看操-欧美18在线-欧美1级

硬件結(jié)構(gòu)主要由5個(gè)部分組成，主控制器STM32f401-Nucleo是四旋翼飛行器的大腦控制整個(gè)系統(tǒng)，借助WiFi—EMW3080模塊進(jìn)行通信，MPU6050作為獲取四旋翼飛行器姿態(tài)信息的傳感器，四個(gè)無刷電機(jī)作為四旋翼飛行器的執(zhí)行器，四旋翼機(jī)架采用450機(jī)架，其結(jié)構(gòu)圖如下圖所示：

1、STM32F401RET6微控制器，基于ARM Cortex-M4處理器，帶DSP，最高支持84MHz主頻

2、支持Arduino UNO R3 Shield擴(kuò)展板，微控制器所有IO口引腳通過排針座引出ST-LINK/V2-1調(diào)試器，支持對(duì)外部微控制器調(diào)試3個(gè)LED；一個(gè)USB通訊LED、一個(gè)電源LED、一個(gè)用戶LED；兩個(gè)機(jī)械按鍵：復(fù)位、用戶USB接口的3個(gè)不同功能：虛擬串口、容量存儲(chǔ)、調(diào)試接口

3、3種不同供電方式：mini USB接口供電、IO引腳用電、通過Arduino UNO R3 Shield接口供電

4、支持Keil、IAR、embed在線IDE的設(shè)計(jì)工具

5、STM32F401 Nucleo開發(fā)板包含了STM32F系列板卡慣有的機(jī)械按鍵、LED指示燈、mini USB調(diào)試接口，眾多IO口外設(shè)通過排針座引出等功能，除此之外，也有與眾不同之處，如兼容Arduino Shield接口，并且可以通過Arduino Shield擴(kuò)展接口給板卡供電，板卡搭載了STM32F401RET6核心微控制器，基于32位的高性能ARM Cortex-M4處理器，帶FPU單元，最高能支持84MHz主頻，見下圖。

電調(diào)采用大唐宏運(yùn)30A

無刷電機(jī)采用2212/920KV

無線模塊采用EMW3080

1、EMW3080是單3.3V供電的、集成Wi-Fi和Cortex-M4F MCU的嵌入式Wi-Fi模塊，最高支持133M主頻和256K RAM，強(qiáng)大的浮點(diǎn)運(yùn)算。EMW3080(B)：無內(nèi)部加密芯片，Memory、外設(shè)接口資源豐富，能滿足大部分應(yīng)用需求和多云的要求。

四旋翼飛行器控制系統(tǒng)是一個(gè)對(duì)耦合度要求高、欠驅(qū)動(dòng)、需要較高的控制準(zhǔn)確性與實(shí)時(shí)性的控制系統(tǒng)，四旋翼飛行器控制系統(tǒng)在姿態(tài)方面主要由橫滾角(Roll)、俯仰角(Pitch)、偏航角(Yaw)、三軸角速度與油門(thrust)這幾個(gè)控制量體現(xiàn)，四旋翼飛行器的整個(gè)控制系統(tǒng)采用串級(jí)PID控制系統(tǒng)，，如下圖所示，內(nèi)環(huán)控制角速度，用于對(duì)飛行器的三個(gè)軸的角速度控制，外環(huán)控制角度，用于對(duì)飛行器的姿態(tài)控制。利用四旋翼四個(gè)電機(jī)各姿態(tài)角分量疊加關(guān)系得出PWM波輸出值來控制每個(gè)電機(jī)轉(zhuǎn)動(dòng)速度。

下圖為利用simulink對(duì)四旋翼飛行器做的仿真框圖，采用的是串級(jí)PID控制。

利用Matlab的GUI界面設(shè)計(jì)做的一個(gè)可視化仿真調(diào)試界面進(jìn)行相應(yīng)的仿真調(diào)試，如下圖所示，其中Ch1、Ch2、Ch3和Ch4通道分別對(duì)應(yīng)四旋翼roll、pitch、thrust和yaw的設(shè)定。當(dāng)設(shè)定四旋翼向右翻滾時(shí)，四旋翼繞x軸旋轉(zhuǎn)的方向增大，如下圖Gryroscope窗口下的藍(lán)色曲線。加速度由突然增大到趨于平穩(wěn)來進(jìn)行自平穩(wěn)校準(zhǔn)，如Accelerometer窗口下的紅色曲線和綠色曲線。

Nucleo模塊程序采用Mbed開發(fā)，數(shù)據(jù)采集計(jì)算以及PWM賦值主要在定時(shí)器中斷中執(zhí)行，main函數(shù)代碼如下：

int main(void)

{

  SystemInit();        //系統(tǒng)初始化

delay_ms(50);

InitMPU6050();   //MPU6050初始化

delay_ms(50);

Get_offsets();  //機(jī)體坐標(biāo)矯正

   for(pre_cnt=0;pre_cnt<FILTER_NUM;pre_cnt++)

   {

     Prepare_Data();

   }

while(1)

{

}  

 }

姿態(tài)傳感器進(jìn)行的濾波處理算法采用互補(bǔ)濾波，代碼如下：

void imuUpdate(Axis3f acc, Axis3f gyro, state_t *state , float dt) /*數(shù)據(jù)融合 互補(bǔ)濾波*/

{

float normalise;

float ex, ey, ez;

float q0s, q1s, q2s, q3s; /*四元數(shù)的平方*/

static float R11,R21;  /*矩陣(1,1),(2,1)項(xiàng)*/

static float vecxZ, vecyZ, veczZ; /*機(jī)體坐標(biāo)系下的Z方向向量*/

float halfT =0.5f * dt;

Axis3f tempacc =acc;

gyro.x = gyro.x * DEG2RAD; /* 度轉(zhuǎn)弧度 */

gyro.y = gyro.y * DEG2RAD;

gyro.z = gyro.z * DEG2RAD;

/* 某一個(gè)方向加速度不為0 */

if((acc.x != 0.0f) || (acc.y != 0.0f) || (acc.z != 0.0f))

{

/*單位化加速計(jì)測(cè)量值*/

normalise = invSqrt(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z);

acc.x *= normalise;

acc.y *= normalise;

acc.z *= normalise;

/*加速計(jì)讀取的方向與重力加速計(jì)方向的差值，用向量叉乘計(jì)算*/

ex = (acc.y * veczZ - acc.z * vecyZ);

ey = (acc.z * vecxZ - acc.x * veczZ);

ez = (acc.x * vecyZ - acc.y * vecxZ);

/*誤差累計(jì)，與積分常數(shù)相乘*/

exInt += Ki * ex * dt ;  

eyInt += Ki * ey * dt ;

ezInt += Ki * ez * dt ;

/*用叉積誤差來做PI修正陀螺零偏，即抵消陀螺讀數(shù)中的偏移量*/

gyro.x += Kp * ex + exInt;

gyro.y += Kp * ey + eyInt;

gyro.z += Kp * ez + ezInt;

}

/* 一階近似算法，四元數(shù)運(yùn)動(dòng)學(xué)方程的離散化形式和積分 */

q0 += (-q1 * gyro.x - q2 * gyro.y - q3 * gyro.z) * halfT;

q1 += (q0 * gyro.x + q2 * gyro.z - q3 * gyro.y) * halfT;

q2 += (q0 * gyro.y - q1 * gyro.z + q3 * gyro.x) * halfT;

q3 += (q0 * gyro.z + q1 * gyro.y - q2 * gyro.x) * halfT;

/*單位化四元數(shù)*/

normalise = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);

q0 *= normalise;

q1 *= normalise;

q2 *= normalise;

q3 *= normalise;

/*四元數(shù)的平方*/

q0s = q0 * q0;

q1s = q1 * q1;

q2s = q2 * q2;

q3s = q3 * q3;

R11 = q0s + q1s - q2s - q3s; /*矩陣(1,1)項(xiàng)*/

R21 = 2 * (q1 * q2 + q0 * q3); /*矩陣(2,1)項(xiàng)*/

/*機(jī)體坐標(biāo)系下的Z方向向量*/

vecxZ = 2 * (q1 * q3 - q0 * q2);/*矩陣(3,1)項(xiàng)*/

vecyZ = 2 * (q0 * q1 + q2 * q3);/*矩陣(3,2)項(xiàng)*/

veczZ = q0s - q1s - q2s + q3s; /*矩陣(3,3)項(xiàng)*/

if (vecxZ>1) vecxZ=1;

if (vecxZ<-1) vecxZ=-1;

/*計(jì)算roll pitch yaw 歐拉角*/

state->attitude.pitch = -asinf(vecxZ) * RAD2DEG;

state->attitude.roll = atan2f(vecyZ, veczZ) * RAD2DEG;

state->attitude.yaw = atan2f(R21, R11) * RAD2DEG;

if (!isCalibrated) /*校準(zhǔn)*/

{

baseZacc = tempacc.x* vecxZ + tempacc.y * vecyZ + tempacc.z * veczZ;

isCalibrated = true;

}

state->acc.z= tempacc.x* vecxZ + tempacc.y * vecyZ + tempacc.z * veczZ - baseZacc; /*Z軸加速度(去除重力加速度)*/

}

Andorid控制界面: