【導讀】畢業(yè)設(shè)計（論文）?；贏RMCORTEX處理器的四旋。翼機姿態(tài)控制系統(tǒng)設(shè)計

　　

【正文】 ata flash 共用一個 SPI 接口，由于 data flash 是高速 SPI，而 MPU6000 最大速度才能達到 1MHz，所以在系統(tǒng)初始化時將 data flash 運用 STM32 內(nèi)部 SPI 進行配置， MPU6000 的 SPI 用 GPIO 去模擬。 1) SPI 的寫指令 MPU6000 按一次 8 位進行傳輸，所以 用戶 只需 對 MOSI 進行 8 次的循環(huán)動作，而由于該 SPI 是上升沿輸出模式，所以在對 MOSI 進行相應(yīng)位的傳輸?shù)臅r候，給 SPI 的時鐘信號一個由低到高的 跳變 信號， 從機便可以接收到 STM32 輸出的電平信號。 2) SPI 的讀取指令 SPI 為下降沿采樣模式，在對 MISO 進行相應(yīng)位的采用時，給 SPI 的時鐘信號一個由高到低的跳變信號， STM32 便可以接收到從機的輸出的電平信號。 3) STM32 向 MPU6000 的 地址寫入一個字節(jié)的數(shù)據(jù) MPU6000 內(nèi)部寄存器的地址最高位為 0 表示寫命令，所以在片選信號拉低時，將要寫入數(shù)據(jù)的地址寫入 到 MPU6000，緊接著寫入相應(yīng)數(shù)據(jù)，將片選信號拉高，完成操作。 4) STM32 從 MPU6000 的地址讀取一個字節(jié)的數(shù)據(jù) MPU6000 內(nèi)部寄存器的地址最高位為 1 表示寫命令，所以在片選信號拉低時，將要讀取數(shù)據(jù)的地址的最高位置一并寫入到 MPU6000，緊接著讀取相應(yīng)數(shù)據(jù)，將片選信號拉高，完成操作。 (2) MPU6000 初始化 21 表 53 MPU6000 初始化 相關(guān) 寄存器 寄存 器名稱 用戶訪問 地址 寄存器描述 PWR_MGMT_1 可讀 /可寫 0x6B 電壓控制 USER_CTRL 可讀 /可寫 0x6A 功能使能 /失能 SMPLRT_DIV 可讀 /可寫 0x19 陀螺儀采樣率 CONFIG 可讀 /可寫 0x1A 低通濾波器頻率 GYRO_CONFIG 可讀 /可寫 0x1B 陀螺儀測量范圍 ACCEL_CONFIG 可讀 /可寫 0x1C 加速度測量范圍 表 41 是 MPU6000 初始化的相關(guān)寄存器， MPU6000 剛上電時處在休眠狀態(tài)，需要對 MPU6000 進行一系列操作 才能正常操作。首先，對 PWR_MGMT_1 的 bit6 位清零，解除休眠模式，接著對 PWR_MGMT_1 的 bit7 位置 1， MPU6000 進行復位，緊接著通過將 USER_CTRL 的 bit5 清零將 I2C 通信關(guān)閉，最后根據(jù)用戶需要設(shè)置 SMPLRT_DIV、CONFIG、 GYRO_CONFIG 等寄存器來配置 MPU6000 的陀螺儀采樣率、低通濾波器頻率、陀螺儀測量范圍 后芯片就正常工作，用戶就可以讀取陀螺和加速度的值了。 (3)陀螺 角 速度 和加速度 的 轉(zhuǎn)換 表 54 MPU6000 陀螺和加速度 寄存器 寄存器名稱 用戶訪問 地址 寄存器描述 ACCEL_XOUT_H 只讀 0x3B X 軸加速度高八位 ACCEL_XOUT_L 只讀 0x3C X 軸加速度低八位 ACCEL_YOUT_H 只讀 0x3D Y 軸加速度高八位 ACCEL_YOUT_L 只讀 0x3E Y 軸加速度低八位 ACCEL_ZOUT_H 只讀 0x3F Z 軸加速度高八位 ACCEL_ZOUT_L 只讀 0x40 Z 軸加速度低八位 GYRO_XOUT_H 只讀 0x43 X 軸陀螺高八位 GYRO_XOUT_L 只讀 0x44 X 軸陀螺低八位 GYRO_YOUT_H 只讀 0x45 Y 軸陀螺高八位 GYRO_YOUT_L 只讀 0x46 Y 軸陀螺低八位 GYRO_ZOUT_H 只讀 0x47 Z 軸陀螺高八位 GYRO_ZOUT_L 只讀 0x48 Z 軸陀螺低八位 22 如表 41 所示為 MPU6000 陀螺和加速度的相關(guān) 寄存器 ， MPU6000 讀取陀螺 或者加速度 的值的操作 如下所示： 首先，用戶需先讀取需要參數(shù)的高字節(jié)數(shù)據(jù)， MCU 將要讀取數(shù)據(jù)的地址最高位置 1 并通過 SPI 發(fā)送給 MCU6000， SPI 片選信號繼續(xù)拉低， MCU 繼續(xù)寫一個字節(jié)的隨機數(shù)據(jù)給 MCU6000， 由于 SPI 全雙工模式工作的，所以 MCU 在寫入數(shù)據(jù)的同時已經(jīng)將所要讀取的數(shù)據(jù)讀取到 MCU 中了，讀取之后片選信號拉高。接著用戶已同樣的方式讀取需要參數(shù)的低字節(jié)，并將高字節(jié)和低字節(jié)數(shù)據(jù)合成一個十六位的數(shù)據(jù)，并通過運算轉(zhuǎn)換成用戶需要得到的參數(shù)。 1)陀螺角速度的轉(zhuǎn)換 下面以陀螺儀初始化時設(shè)定量程為 177。2021176。 /s 為例，介紹陀螺角速度的轉(zhuǎn)換：在開機時 STM32 通過讀取陀螺儀相應(yīng)地址的高 8 位和低 8 位的的值進行合成一個 16 位的數(shù)據(jù)，為了獲得更準確的數(shù)據(jù)，可以讀取 100 次得到 100 個 16 位的數(shù)據(jù)，再對這些數(shù)據(jù)求和取平均值， 得到 0 點偏移量，在采樣時，每隔 20ms 去采樣一次陀螺儀地址的數(shù)據(jù)，合成一個新的 16 位的數(shù)據(jù)，將該數(shù)據(jù)與 0 點偏移量進行求差， 根據(jù)陀螺儀初始化的值 將該差值除以相應(yīng)的分辨率 176。/S，就得到采集出來的角速度了，再將該速度 轉(zhuǎn)換成弧度 /s，陀螺儀角速度采樣完成。陀螺儀所設(shè)定的量程和相應(yīng)的分辨率如下 表 所示： 表 55 陀螺儀的量程和分辨率 FS_SEL 角速度量程 角速度分辨率 0 ? 250176。 /s 131 LSB/176。 /s 1 ? 500176。 /s LSB/176。 /s 2 ? 1000176。 /s LSB/176。 /s 3 ? 2021176。 /s LSB/176。 /s 2)加速度的轉(zhuǎn)換 以加速度初始化時設(shè)定量程為 177。2G 為例，介紹加速度的轉(zhuǎn)換：在開機時 STM32 通過讀取 加速度 相應(yīng)地址的高 8 位和低 8 位的的值進行合成一個 16 位的數(shù)據(jù)， 同樣 可以讀取100 次得到 100 個 16 位的數(shù)據(jù)， 對這些數(shù)據(jù)求和取平均值，得到 0 點偏移量，在采樣時，每隔 20ms 去采樣一次 加速度 地址的數(shù)據(jù)，合成一個新的 16 位的數(shù)據(jù)，將該數(shù)據(jù)與 0 點偏移量進行求差， 并 根據(jù) 加速度 初始化的值將該差值除以相應(yīng)的分辨率，就得到采集出來的 加 速度了， 加速度 采樣完成。 特別注意的是：加速度的 Z 軸受到重力的影響，所以在計算加速度 Z 軸求差之后還要加上一個重力的基準值，才是最終測得的加速度 Z 軸的值。加速度所設(shè)定的量程和相應(yīng)的分辨率如下圖所示： 23 表 56 加速度 的量程和分辨率 AFS_SEL 加速度量程 加速度分辨率 0 ? 2g 16384 LSB/g 1 ? 4g 8192 LSB/g 2 ? 8g 4096 LSB/g 3 ? 16g 2048 LSB/g 姿態(tài)信息的 解 算程序設(shè)計 姿態(tài)信息的解算 的目的是 將陀螺儀角速度和加速度進行融合，計算出 計算機系統(tǒng)方便使用的歐拉角 (pitch、 roll、 yaw)。本設(shè)計采用的是傳統(tǒng)的，技術(shù)相對成熟的四元數(shù)法。運用四元數(shù)法，可以將陀螺儀和加速度計得出的數(shù)值轉(zhuǎn)換成四維空間，運用四維空間的乘法來表示三 維空間的旋轉(zhuǎn)， 減輕了計算機的復雜的算法，加快了反應(yīng)速度。 使用四元數(shù)函數(shù)時，只要輸入已經(jīng)采集過來的陀螺儀和加速度的六個參數(shù)，并設(shè)定好時候的捕獲時間，就可以解算出四元數(shù)了，最后將四元數(shù)轉(zhuǎn)換成歐拉角。 姿態(tài)控制的程序設(shè)計 姿態(tài)控制的流程 四旋翼飛行器的姿態(tài)控制系統(tǒng)是通過 PID 的控制思想，將姿態(tài)的給定值和傳感器的測出值的偏差反饋給電機，實現(xiàn)電機的聯(lián)動調(diào)節(jié)，保證四旋翼機的平穩(wěn)飛行。四旋翼機姿態(tài)控制的程序流程圖 52 所示。 24 圖 52 姿態(tài)控制的總流程 姿態(tài)的 PID 控制程序設(shè)計 姿態(tài)控制系統(tǒng) PID 程序設(shè)計是本設(shè)計中最重要的環(huán)節(jié)。 四旋翼飛行器的姿態(tài)控制過程屬于自動化領(lǐng)域的過程控制。對 于 姿態(tài)控制系統(tǒng)來說 ，我們 要控制他，目前的理論是引入負反饋 ， PID 控制器的作用就是將機身的姿態(tài)輸出經(jīng)過傳感器的測量變送，轉(zhuǎn)化成可以控制的方向角，這個方向角再和我們預(yù)先給定的姿態(tài)方向角進行計算和處理，使得輸出姿態(tài)跟隨輸入姿態(tài)的變化而變化，做到系統(tǒng)的無差調(diào)節(jié)，即讓輸出姿態(tài)信息等于輸入姿態(tài)信息。 四旋翼飛行器的簡單控制系統(tǒng)的結(jié)構(gòu)圖如圖 53 所示： Y N N Y Y N Y Y N N Y Y N N N Y Y Y Y N N Y Y N 開始 四電機同步聯(lián)動平穩(wěn)飛行 姿態(tài)偏差 ？ 偏航？ 俯仰？ 左偏？ 4 電機加速 3 電機減速 上仰？ 2 加速 4 減速 2 減速 4 加速 4 加速 3 減速 3 加速 4 減速 偏差？ 偏差？ 偏差？ 對角線電機同步聯(lián)動 4 電機加速 3 電機減速 左 滾轉(zhuǎn)？ 25 圖 53 四旋翼飛行器的過程控制框圖 姿態(tài)比例控制程序設(shè)計 比例調(diào)節(jié) (簡稱 P 調(diào)節(jié) )中，調(diào)節(jié) 器的輸出姿態(tài)信號和輸入姿態(tài)信號的偏差成比例關(guān)系。當系統(tǒng)出現(xiàn)誤差時， P 調(diào)節(jié)器第一時間反應(yīng)過來，反饋給輸入姿態(tài)，來補償系統(tǒng)偏差。也就是說，當傳感器測量出來的姿態(tài)信息量與我們給定姿態(tài)信息的偏差不為 0 時，當前誤差多少，我們就給出多少輸出值來補償當前誤差。 姿態(tài)比例控制程序的設(shè)計很簡單，只需要將慣性測量單元測出來的姿態(tài) 橫滾角、俯仰角 、偏航角 和我們希望給定的姿態(tài)角 分別 求差，再將差值 分別 乘上一個 不同的 比例系數(shù)，就得到了 三個 比例控制量， 再 將 這三個個 比例 控制量通過 飛行器的控制機制賦值給四路電機。 姿態(tài) 積分 控制程序設(shè)計 姿態(tài)的積分控制可以提高姿態(tài)控制系統(tǒng)的無差度。比例調(diào)節(jié)方式的特點是 快速而有勁，但是比例調(diào)節(jié) 不可避免地會使系統(tǒng)存在穩(wěn)態(tài)誤差，因為比例環(huán)節(jié)屬于有差調(diào)節(jié)。所以在姿態(tài)控制系統(tǒng)中有必要引入積分控制環(huán)節(jié) 來消除靜差 。在數(shù)字控制系統(tǒng)中，只要將測量單元檢測出來的姿態(tài)橫滾角、俯仰角、偏航角和上一次保存下來的姿態(tài)角進行不斷的累加，再將這個值乘上一個不同 的積分系數(shù)，就得到了三個積分控制量，并通過飛行器的控制機制賦值換算成 PWM 賦值給四路電機。 需要注意的是，這種累加無疑會造成系統(tǒng)的不穩(wěn)定，如果系統(tǒng)長時間處于不平衡位置， 由于實際姿態(tài)角與給定姿態(tài)角不斷的隨時間的累計而累加，積分的作用會變得越來越強，最后超出比例控制作用，使姿態(tài)控制系統(tǒng)崩潰。所以在程序設(shè)計時必須要給定一個小的角度范圍內(nèi)進行積分項的計算，超出這個范圍后，系統(tǒng)不再對姿態(tài)角進行積分處理，同時，也要對積分項進行限幅處理。 姿態(tài) 微分 控制程序設(shè)計 微分調(diào)節(jié)器是一種可預(yù)見性的調(diào)節(jié)器，這種調(diào)節(jié)器可以提前預(yù)知系統(tǒng) 的運動趨勢，— 姿態(tài)輸入 姿態(tài)輸出 PID 調(diào)節(jié)器 驅(qū)動電機 四旋翼飛行器 傳感器的測量 26 微分 調(diào)節(jié)器和積分調(diào)節(jié)器具有一定的互補性質(zhì)，積分調(diào)節(jié)器具有一定的滯后性，而微分調(diào)節(jié)器具有抑制滯后的功能，使姿態(tài)控制系統(tǒng)變得非常敏感。為了簡便系統(tǒng)的算法，本設(shè)計巧妙的運用了慣性測量單元自身的自增效應(yīng)，直接利用慣性測量單元測量出來的陀螺儀角速度的值來代替微分項，使得整個控制系統(tǒng)的算法更加簡潔， 反應(yīng)更加快速。 姿態(tài)的控制機制 將姿態(tài)控制的 P 控制量、 I 控制量和 D 控制量進行疊加就得到了三個方向角的 PID的控制變量，得到了姿態(tài)控制的 PID 控制變量，還需要了解如何將這些控制變量轉(zhuǎn)換成四路電機的 PWM 控制信號量。 表 57 詳細說明了四旋翼飛行器的控制機制 (其中“ +”代表加速，“ ”代表減速 )： 表 57 四旋翼飛行器的控制機制 電機 1(M1) 電機 2(M2) 電機 3(M3) 電機 4(M4) 橫滾 (ele) + + 俯仰 (ail) + + 偏航 (rud) + + 從上表不難得出以下公式： M1 = + ele + ail + rud。 M2 = ele + ail rud。 M3 = ele ail + rud。 M4 = + ele ail rud。 將三個方 向角的控制變量賦值給 ele， ail， rud 就能得到四路電機的 PWM 控制量，最后將 M M M M4 同時加上一個相同的油門值就能得到 四路 PWM 信號來驅(qū)動電機。 6 系統(tǒng)調(diào)試 系統(tǒng)的硬件調(diào)試 (1)出現(xiàn)的問題 ， 出現(xiàn)電源 5V 電壓供電時，沒有 電源的指示燈沒量，用萬用表測量各個引腳電壓時，測量出來的值如表： 27 表 61 TPS79133 各個引腳電壓 引腳號 電壓 1(IN) 2(GND) 3(EN) 4(BYPASS) 300MV 5( 輸出 ) 解決方法：查找 TPS79133 相關(guān)數(shù)據(jù)手冊，發(fā)現(xiàn)手上擁有的 TPS79133 的第 3 引腳是高電平時第五引腳才有