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