四旋翼飛行器的位姿控制器的制造方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明是一種四旋翼飛行器的位姿控制器，可對(duì)四旋翼位姿(位置和姿態(tài))進(jìn)行控 制，進(jìn)而完成路點(diǎn)飛行、懸停等飛行任務(wù)，屬于飛行器控制的技術(shù)領(lǐng)域。
【背景技術(shù)】
[0002] 無(wú)人駕駛飛機(jī)(Unmanned Aerial Vehicle)是一種以無(wú)線電遙控或由自身程序控 制的不載人飛機(jī)，與載人飛機(jī)相比，它具有體積小、造價(jià)低、對(duì)飛行環(huán)境要求低、空中生存能 力強(qiáng)等特點(diǎn)。不僅可以節(jié)省大量的人力和物力資源，而且使用起來(lái)更加安全方便，這些獨(dú)特 的優(yōu)勢(shì)使無(wú)人機(jī)在工農(nóng)業(yè)生產(chǎn)和軍事領(lǐng)域備受青睞，應(yīng)用前景廣闊。
[0003] 四旋翼(Quadrotor)是一類(lèi)典型的無(wú)人駕駛的旋翼式飛行器，由4個(gè)圍繞機(jī)體中心 對(duì)稱(chēng)分布的旋翼構(gòu)成的飛行器。它是一種結(jié)構(gòu)簡(jiǎn)單、可以垂直起降，它在總體布局形式上屬 于非共軸式碟形飛行器，具有獨(dú)特的飛行控制方式。同其他結(jié)構(gòu)的直升機(jī)相比，四旋翼直升 機(jī)的主要有以下兩點(diǎn)： (1) 有效載荷更大，與常規(guī)旋翼式飛行器相比，四只旋翼使它可以產(chǎn)生更大的升力，因 此擁有更大的載重量； (2) 控制系統(tǒng)簡(jiǎn)單，控制四個(gè)旋翼的轉(zhuǎn)速就可以實(shí)現(xiàn)姿態(tài)、水平/垂直移動(dòng)，不需要配備 專(zhuān)門(mén)的反扭矩槳，四只旋翼可互相抵消各自產(chǎn)生的反扭力矩。
[0004] 近年來(lái)，四旋翼的應(yīng)用和研究廣泛受到廣泛重視。在軍事領(lǐng)域，四旋翼可作為空中 偵察平臺(tái)執(zhí)行偵察監(jiān)視、目標(biāo)定位、激光制導(dǎo)、通信中繼、電子干擾、戰(zhàn)斗評(píng)估等任務(wù)。在民 用領(lǐng)域，四旋翼可應(yīng)用于場(chǎng)區(qū)監(jiān)控、電力線路巡查、氣象探測(cè)、公路巡視、勘探測(cè)繪、電影特 技、航空攝影、交通管理、森林火災(zāi)救防等。

【發(fā)明內(nèi)容】

[0005] 本發(fā)明提出的是一種四旋翼飛行器的位姿控制器，選用ARM芯片作為主控芯片，設(shè) 計(jì)一種硬件結(jié)構(gòu)簡(jiǎn)單、易于實(shí)現(xiàn)、抗干擾能力強(qiáng)的四旋翼位姿控制器，使得四旋翼能夠勝任 多種飛行任務(wù)。該控制器的構(gòu)造要點(diǎn)包括采用姿態(tài)角和姿態(tài)角速度雙反饋回路，特別設(shè)計(jì) 了規(guī)劃模塊(在主控芯片中軟件方式實(shí)現(xiàn)），根據(jù)飛行器的飛行狀態(tài)和飛行任務(wù)要求，適時(shí) 調(diào)節(jié)雙PID控制器輸出。
[0006] 本發(fā)明的技術(shù)解決方案：四旋翼飛行器的位姿控制器，其特征是包括2個(gè)反饋控制 回路，姿態(tài)角度控制器、姿態(tài)角速度控制器，規(guī)劃模塊，控制量融合模塊;所述2個(gè)反饋控制 回路是姿態(tài)角度和姿態(tài)角速度這2個(gè)3維向量回路;所述姿態(tài)角度控制器和姿態(tài)角速度控制 器采用PID控制算法，分別控制四旋翼飛行器的姿態(tài)角度和姿態(tài)角速度，規(guī)劃模塊根據(jù)四旋 翼機(jī)當(dāng)前飛行狀態(tài)和飛行任務(wù)要求，調(diào)節(jié)這兩類(lèi)控制器輸出權(quán)值和被控量期望值，最后再 由控制量融合模塊計(jì)算出四個(gè)旋翼電機(jī)的最終控制量。
[0007] 本發(fā)明的優(yōu)點(diǎn)： (1)該控制器能完成路點(diǎn)飛行、懸停等多種飛行任務(wù)，抗干擾能力較強(qiáng)； (2) 該控制器主要用程序軟實(shí)現(xiàn)，硬件結(jié)構(gòu)簡(jiǎn)單，易于實(shí)現(xiàn)，成本低廉，運(yùn)行可靠； (3) 本控制器程序均采用模塊化設(shè)計(jì)，具有較好的可移植性，縮短了系統(tǒng)軟硬件的開(kāi)發(fā) 周期。
【附圖說(shuō)明】
[0008]圖1是四旋翼坐標(biāo)系示意圖。
[0009]圖2是四旋翼飛行器的位姿控制器結(jié)構(gòu)圖。
[0010]圖3是角度和角速度控制器結(jié)構(gòu)示意圖。
[0011]圖4是控制量融合模塊結(jié)構(gòu)示意圖。
[0012]圖5是控制器功能結(jié)構(gòu)示意圖。
【具體實(shí)施方式】
[0013] 四旋翼飛行器的位姿控制器，其結(jié)構(gòu)包括2個(gè)反饋控制回路，姿態(tài)角度控制器、姿 態(tài)角速度控制器，規(guī)劃模塊，控制量融合模塊;所述2個(gè)反饋控制回路是姿態(tài)角度和姿態(tài)角 速度這2個(gè)3維向量回路;所述姿態(tài)角度控制器和姿態(tài)角速度控制器采用PID控制算法，分別 控制四旋翼飛行器的姿態(tài)角度和姿態(tài)角速度，規(guī)劃模塊根據(jù)四旋翼機(jī)當(dāng)前飛行狀態(tài)和飛行 任務(wù)要求，調(diào)節(jié)這兩類(lèi)控制器輸出權(quán)值和被控量期望值，最后再由控制量融合模塊計(jì)算出 四個(gè)旋翼電機(jī)的最終控制量。
[0014] 下面結(jié)合附圖進(jìn)一步描述本發(fā)明的技術(shù)方案： 如圖1所示，全局坐標(biāo)系足H，四旋翼機(jī)體坐標(biāo)系:-￡益，四旋翼的參考點(diǎn)選為機(jī)體中 心位置，在中的坐標(biāo)記為(H，該點(diǎn)與恐2的坐標(biāo)原點(diǎn)重合。其中，軸方向?yàn)?四旋翼機(jī)頭方向，繞尤軸旋轉(zhuǎn)的角度記為e，稱(chēng)為滾轉(zhuǎn)角；繞了軸旋轉(zhuǎn)的角度記為逆，稱(chēng)為 俯仰角；繞:?:軸旋轉(zhuǎn)的角度記為少，稱(chēng)為偏轉(zhuǎn)角。四旋翼位姿的向量形式記為
[0015] 如圖2所示，四旋翼飛行器的位姿控制器，其結(jié)構(gòu)是包含2個(gè)反饋控制回路，同時(shí)角 度控制器、角速度控制器分別控制四旋翼的角度和角速度，規(guī)劃模塊根據(jù)四旋翼當(dāng)前飛行 狀態(tài)和飛行任務(wù)要求，調(diào)節(jié)控制器權(quán)值和控制器期望值，最后再由控制量融合模塊通過(guò)加 權(quán)計(jì)算出四個(gè)旋翼電機(jī)的最終控制量，由于受外界不確定因素如風(fēng)、氣流的干擾，當(dāng)姿態(tài)角 偏離期望值幅度過(guò)大時(shí)，應(yīng)優(yōu)先進(jìn)行角度控制，相應(yīng)提高權(quán)重值心:；反之，當(dāng)姿態(tài)角處于正 常范圍，姿態(tài)角速度偏離期望值幅度過(guò)大時(shí)，應(yīng)優(yōu)先進(jìn)行角速度控制，相應(yīng)提高權(quán)重值％。
[0016] 豐1 7\7始制典給山切甫豐

規(guī)劃模塊根據(jù)四旋翼當(dāng)前飛行狀態(tài)和飛行任務(wù)要求，確定角度和角速度控制器權(quán)重和 控制器期望值。如表1所示，起飛/降落，懸停和路點(diǎn)飛行狀態(tài)可根據(jù)遙控器/地面控制站的 命令加以識(shí)別；飛行過(guò)程中對(duì)角速度采樣值的絕對(duì)值進(jìn)行滑動(dòng)平均(滑動(dòng)窗中含128個(gè)點(diǎn)）， 如果角速度均值大于120，則飛行器處于振動(dòng)狀態(tài)，規(guī)劃模塊將修改雙控制器的權(quán)重;反之， 飛行器處于穩(wěn)定懸停狀態(tài)或路點(diǎn)飛行狀態(tài)。路點(diǎn)飛行狀態(tài)需外接GPS模塊構(gòu)成反饋通道，規(guī) 劃模塊根據(jù)位置偏差確定姿態(tài)角期望值，進(jìn)而完成路點(diǎn)飛行任務(wù)。
[0017] 所述的角度控制器的信號(hào)輸出端接控制量融合模塊的第一信號(hào)輸入端，規(guī)劃模塊 的第一信號(hào)輸出端接角度控制器的信號(hào)輸入端，規(guī)劃模塊的第二信號(hào)輸出端接控制量融合 模塊的第二信號(hào)輸入端，規(guī)劃模塊的第三信號(hào)輸出端接角速度控制器的信號(hào)輸入端，角速 度控制器的信號(hào)輸出端接控制量融合模塊的第三信號(hào)輸入端，控制量融合模塊的信號(hào)輸出 端接四旋翼飛行器的信號(hào)輸入端，四旋翼飛行器的信號(hào)輸出端接姿態(tài)檢測(cè)模塊的信號(hào)輸入 端，姿態(tài)檢測(cè)模塊的第一信號(hào)輸出端接角度控制器的第二信號(hào)輸入端，姿態(tài)檢測(cè)模塊的第 二信號(hào)輸出端接角速度控制器的第二信號(hào)輸入端。
[0018] 如圖3所示，姿態(tài)角度控制器和姿態(tài)角速度控制器均采用位置式離散PID算法，與 滾轉(zhuǎn)、俯仰、偏轉(zhuǎn)角對(duì)應(yīng)，角度控制器由3個(gè)角度的PID控制器組成。角速度控制器與之類(lèi)似， 由3個(gè)角速度的PID控制器組成。姿態(tài)檢測(cè)模塊（mpu6050芯片）可直接檢測(cè)當(dāng)前角速度 玟表少，進(jìn)而運(yùn)用四元數(shù)法解算出當(dāng)前角度故我#。規(guī)劃模塊根據(jù)當(dāng)前飛行任務(wù)要求，給定 角度和角速度期望值多。例如，完成懸停任務(wù)時(shí)，均 為〇;路點(diǎn)飛行任務(wù)時(shí)，初始階段不為〇,設(shè)置毛=?關(guān)閉角速度控制通道。下面以 尤軸方向滾轉(zhuǎn)角名為例，令％ =#-貨，其角度控制器第k個(gè)周期的輸出值為，
(1) 其中，分別為角度PID控制器的比例、積分、微分系數(shù)。同理可得Γ、2:軸方 向角度控制器的輸出值，最終可得
[0019]角速度控制器與角度控制器結(jié)構(gòu)類(lèi)似，仍以:尤軸方向?yàn)槔睿?=#-:#，其角 速度控制器第k個(gè)周期的輸出值為，
(2) 其中，霉Ui:分別為角速度PID控制器的比例、積分、微分系數(shù)。同理可得F、:2軸 方向角度控制器的輸出值，最終可得：
[0020]如圖4所示，控制量融合模塊，規(guī)劃模塊根據(jù)當(dāng)前飛行狀態(tài)，確定合適的權(quán)重值
，調(diào)節(jié)角度和角速度控制器輸出，經(jīng)加權(quán)計(jì)算出基于姿態(tài)角的綜合 控制量g，最后結(jié)合高度控制量alt，計(jì)算出4個(gè)旋翼電機(jī)的控制量。
[00： (3) … 、-- ⑷ 其畔
alt為四旋翼高度控制量。是4個(gè)旋翼 電機(jī)的控制量。
[0022] 如圖5所示，該控制器采用STM32f4076VET芯片為主控芯片。傳感器模塊由氣壓計(jì)、 磁場(chǎng)傳感器、陀螺儀和加速度計(jì)構(gòu)成，其中陀螺儀和加速度計(jì)集成于MPU6050芯片中，用于 檢測(cè)飛行器的姿態(tài)角和姿態(tài)角速度，是飛行器的核心傳感器;氣壓計(jì)MS5611用于測(cè)量飛行 器的高度，磁場(chǎng)傳感器HMC5983用于控制飛行器機(jī)頭方向。遙控器通信接口可外接2.4G無(wú)線 接收器，方便向飛行器發(fā)送命令或進(jìn)行緊急