四軸起飛時，發(fā)出觸發(fā)信號使導航模塊開始工作，同時讀取ICM20602的加速度計、陀螺儀數(shù)據(jù)，對數(shù)據(jù)卡爾曼濾波后姿態(tài)解算，對角度與角速度采取串級PID調(diào)節(jié)。控制系統(tǒng)算法設(shè)計主要有ICM20602濾波算法，姿態(tài)解算算法、串級PID控制算法和定高部分控制算法。礙于篇幅所限，下面介紹最重要的串級PID控制算法和定高部分控制算法。地理坐標系中重力的水平分量為零，僅用三軸陀螺儀和三軸加速度計無法計算出航向角，由于巡線機器人保持穩(wěn)定飛行只需要橫滾角（roll）和俯仰角（pitch），所以四元數(shù)轉(zhuǎn)換成歐拉角。定高控制算法采用的是增量式PID控制，定高控制的輸出最后與姿態(tài)控制的輸出疊加到四個電機的控制中。數(shù)據(jù)濾波使用的是低通濾波，采用近三次的平均值。為了防止姿態(tài)對激光測距的影響及減小高度控制對姿態(tài)控制的干擾使用歐拉角來校正高度值，即Hight=(float)Hight*(cos(roll)* cos(pitch))。將四元數(shù)轉(zhuǎn)換后的歐拉角與陀螺儀測出來的角速度進行串級PID控制，其中歐拉角作為外環(huán)，角速度作為內(nèi)環(huán)。外環(huán)的PID以及內(nèi)環(huán)的PD設(shè)定值為測試數(shù)據(jù)值。由于內(nèi)環(huán)的角速度控制不需要無靜差,所以內(nèi)環(huán)采用PD控制,為防止測量的誤差造成較大影響,外環(huán)積分需要限幅。
標簽：
傳感器
上傳時間：
2022-06-24
上傳用戶：默默
