軸飛行器等系統的姿態解算。
[0047] 本發明在建立了兩輪自平衡智能車系統的運動模型并得到了系統穩定的條件。針 對兩輪自平衡智能車的動態條件下的運行特點，結合慣性傳感器陀螺儀和加速度計的固有 屬性特點，建立了二者的輸出模型并應用卡爾曼濾波器提高姿態解算精度。通過在兩輪自 平衡智能車平臺進行實驗，對比單獨應用陀螺儀和加速度計的數據和應用卡爾曼濾波器進 行濾波融合后的數據，可知本發明對兩輪自平衡智能車的姿態解算精度有著明顯的提高。
【附圖說明】
[0048] 圖1兩輪自平衡智能車結構示意圖；
[0049] 圖2卡爾曼濾波器數據融合輸入輸出關系圖；
[0050] 圖3卡爾曼濾波器遞歸運算流程圖；
[0051] 圖4單獨應用陀螺儀的姿態解算漂移示意圖；
[0052] 圖5a為啟動實驗卡爾曼濾波數據融合、單獨應用加速度計和單獨應用陀螺儀進 行姿態解算對比曲線；
[0053] 圖5b為通過路障實驗卡爾曼濾波數據融合、單獨應用加速度計和單獨應用陀螺 儀進行姿態解算對比曲線；
[0054] 圖5c為停止實驗卡爾曼濾波數據融合、單獨應用加速度計和單獨應用陀螺儀進 行姿態解算對比曲線。
【具體實施方式】
[0055] 本發明公布了一種兩輪自平衡智能車姿態解算方法，同時也適用于兩輪自平衡機 器人。
[0056] 本發明公開了一種兩輪自平衡智能車姿態控制方法，包括如下步驟：
[0057] 步驟1，應用動力學方程建立兩輪自平衡智能車的運動模型；
[0058] 步驟2,在運動模型中引入單位負反饋和串聯ro控制器以調整相應參數；
[0059] 步驟3,應用與智能車剛性連接的慣性傳感器陀螺儀和加速度計構建兩輪自平衡 智能車的姿態測量系統；
[0060] 步驟4,根據慣性傳感器陀螺儀和加速度計的輸出模型，分別對兩輪自平衡智能車 進行靜態和動態條件下特性分析，進而得到各自包含誤差的數據輸出模型；
[0061] 步驟5,應用濾波算法對慣性傳感器陀螺儀和加速度計的數據進行最優估計，從而 實現對兩輪自平衡智能車的姿態解算，并完成對兩輪自平衡智能車的姿態控制。
[0062] 應用牛頓力學方法建立兩輪自平衡智能車的運動模型，兩輪自平衡智能車結構示 意圖如圖1所示，根據系統特點設計控制器進而得到系統的控制方法。
[0063] 步驟1中所述動力學方程為：
[0065] 其中，L為兩輪自平衡智能車的擺桿質心到兩個同軸車輪軸線的距離，g為重力加 速度，Θ⑴為t時刻兩輪自平衡智能車與豎直方向的夾角即姿態傾角，x(t)為t時刻由外 界擾動所引起的兩輪自平衡智能車角加速度，a(t)為t時刻車輪運動產生的加速度。
[0066] 當兩輪自平衡智能車處于自平衡狀態時，在姿態傾角Θ(t) = 0處通過泰勒級數 展開進行線性化處理，則有sin0 (t) =Θ(t)，C〇S0 (t) =1，當智能車處于自平衡狀態時， 運行狀態為靜止狀態或勻速直線運動狀態，則有智能車車輪運動產生的加速度a(t) = 0， 此時對應的動力學方程為：
[0068] 引入負反饋和串聯ro控制器后，對上式進行拉氏變換得到兩輪自平衡智能車在 自平衡狀態下S域內姿態傾角?(S)與外界擾動所引起的角加速度x(s)之間的傳遞函數
[0069] H(s)：
[0071]步驟2中，對運動模型引入單位負反饋和串聯ro控制器后傳遞函數H(s)為：
[0073]其中，KP、KD分別為ro控制器的比例系數和微分系數，對上式使用勞斯判據進行 分析，當滿足以下條件時，兩輪自平衡智能車姿態的自由模態收斂并趨向于零從而達到穩 定：
[0075] 結合慣性傳感器陀螺儀加速度計的固有屬性，對動態條件下的數據輸出分別進行 分析并得到各自的特征模型。
[0076] 步驟4中，慣性傳感器陀螺儀的輸出模型為：
[0077] cogyro=ωreal+εg+εn，
[0078] 其中為慣性傳感器陀螺儀輸出的角速度，ωraal為兩輪自平衡智能車真實的 角速度，％為慢時變漂移誤差，εη由溫度、摩擦力和不穩定力矩構成的隨機誤差，可以近 似的認為是白噪聲；
[0079] 慣性傳感器加速度計輸出模型為：
[0080] 9acce=Θreal+0g,
[0081] 其中θ_表示慣性傳感器加速度計輸出的姿態傾角，Θraal為兩輪自平衡智能車 真實角度，εa為兩輪自平衡智能車運動時產生的動態加速度。
[0082] 如圖2所示，在兩輪自平衡智能車硬件平臺基礎上，應用改進的卡爾曼濾波器對 姿態測量系統的原始數據《gy"、03_進行濾波融合，從而實現對兩輪自平衡智能車的姿態 解算，并完成對兩輪自平衡智能車的姿態控制。
[0083] 如圖3所示，步驟5中，采用如下公式對陀螺儀加速度計數據進行最優估計，包括 步驟3-1~步驟3-7:
[0084] 步驟3-1，對慣性傳感器陀螺儀和加速度計數據進行采樣，更新陀螺儀角速度 ω_〇〇和加速度計傾角Uk+1)。
[0085] 步驟3-2,根據k時刻的最優估計姿態傾角Θ(k|k)、k時刻輸出角速度c〇gyro(k) 和姿態測量系統采樣時間T，完成k+1時刻姿態傾角的估計：
[0086] Θ(k+l|k) =θ(k|k) + 〇gyro(k)T,
[0087] 慣性傳感器陀螺儀積分傾角和真實傾角對比圖如圖4所示；
[0088] 步驟3-3,根據k時刻姿態傾角Θ(k|k)對應的協方差P(k|k)和系統過程噪聲的 協方差Q，更新k+Ι時刻姿態傾角Θ(k+Ι|k)的協方差：
[0089] P(k+1 |k) =P(k|k)+Q,
[0090] 步驟3-4,根據k+Ι時刻姿態傾角Θ(k+11k)的協方差P(k+11k)和系統測量噪聲 協方差R，計算k+1時刻的卡爾曼增益Kg(k+1):
[0092] 步驟3-5,根據當前時刻姿態傾角Θat：ra;(k+l)、姿態傾角估計Θ(k+1 |k)和卡爾曼 增益Kg (k+Ι)計算k+Ι時刻最優估計姿態傾角：
[0093] Θ (k+Ι | k+Ι) = Θ (k+1 |k)+Kg(k+l) ( Θ acce(k+l)-Θ (k+l|k)),
[0094] 步驟3-6,根據k+1時刻的卡爾曼增益Kg (k+1)、Θ(k+11k)對應的協方差 P(k+11k)，更新k+1時刻最優估計姿態傾角的協方差：
[0095]P(k+11k+1) = (1-Kg(k+1))P(k+11k),
[0096] 步驟3-7,根據步驟3-1~步驟3-6進行遞歸運算即可得到姿態傾角的最優估計， 從而實現對兩輪自平衡智能車的姿態解算。
[0097] 實施例：
[0098] 應用自行設計的兩輪自平衡智能車平臺對姿態解算方法予以實施。姿態控制應用 ro控制，MPU6050型數字陀螺儀和MMA7361型模擬加速度計構成了兩輪自平衡智能車的姿 態測量系統并與智能車剛性連接。智能車通過IIC接口獲取陀螺儀輸出的包含由溫度、摩 擦力和不穩定力矩所引起的漂移的姿態信息，通過ADC接口獲取加速度計輸出的包含高頻 動態加速度噪聲的姿態信息。將上述姿態信息單位歸一化后，應用權利要求3所述的解算 方法進行姿態解算獲取姿態傾角的最優估計，其初始狀態（即k= 0時）相關參數如下：
[0099] 積分時間T:0· 005 ;
[0100] Θ(k|k)對應的協方差