一種兩輪自平衡智能車姿態控制方法
【技術領域】
[0001] 本發明屬于慣性導航領域，尤其涉及一種兩輪自平衡智能車姿態控制方法。
【背景技術】
[0002] 兩輪自平衡智能車是一種典型的非線性欠驅動系統，是一種由兩個電機以差分方 式驅動兩個同軸相連的車輪運動的具有自平衡能力的智能車。具有體積小，機動性強的特 點，可實現零轉彎半徑及狹窄通道通過的能力。為了實現兩輪自平衡智能車在運動過程中 處于自平衡狀態，需要對其姿態信息進行準確的實時測量。常用構成姿態測量系統的慣性 傳感器的受溫度、動態加速度噪聲等因素影響，往往不能夠對姿態信息進行全面精確地實 時測量，與此同時，測量誤差還會隨著時間的增大，最終導致系統無法正常工作。姿態解算 的精度對兩輪自平衡智能車的姿態控制的動態特性和控制品質有著較大的影響。因此，針 對兩輪自平衡智能車運動特點并結合相關慣性傳感器的特性進行準確的姿態解算十分重 要。
[0003] 近年來，國內外諸多學者應用Largrange法和牛頓-歐拉法建立了兩輪自平衡智 能車的模型，并運用了多種不同控制理論完成了控制器的設計。在進行控制器設計仿真實 驗時往往將智能車的姿態信息進行理想化處理，沒有結合實際兩輪自平衡智能車的系統特 點以及慣性傳感器的固有屬性進行研究。例如：慣性傳感器陀螺儀具有較好的動態特性，但 是由諸多因素所引起的累積漂移誤差，且進行姿態解算獲取姿態傾角的過程需要角速度對 時間積分，因此會存在一定的姿態解算滯后；加速度計傳感器具有靈敏度高，相對陀螺儀漂 移誤差小的固有屬性，但是由運動引起的動態加速度高頻噪聲的特性決定了其不適合跟蹤 過動態角度運動，因此單獨的應用加速度計傳感器進行姿態解算并不合適。
【發明內容】
[0004] 本發明主要解決的技術問題是針對上述【背景技術】的不足，根據兩輪自平衡智能車 運行對姿態解算精度和姿態控制的需求提出了一種兩輪自平衡智能車姿態控制方法，達到 了提高兩輪自平衡智能車姿態解算精度的目的。慣性傳感器陀螺儀具有動態性能良好，不 受動態加速度變化的影響的特點，但是存在由溫度、摩擦力和不穩定力矩引起的漂移誤差； 慣性傳感器加速度計在靜態條件下具有較好的檢測效果，但是在動態條件下受動態加速度 影響較大。本發明的目的是為了克服現有單一慣性傳感器固有屬性的不足，提出了一種兩 輪自平衡智能車姿態控制方法。
[0005] 本發明公開了一種兩輪自平衡智能車姿態控制方法，包括如下步驟：
[0006] 步驟1，應用動力學方程建立兩輪自平衡智能車的運動模型；
[0007] 步驟2,在運動模型中引入單位負反饋和串聯ro控制器以調整相應參數；
[0008] 步驟3,應用與智能車剛性連接的慣性傳感器陀螺儀和加速度計構建兩輪自平衡 智能車的姿態測量系統；
[0009] 步驟4,根據慣性傳感器陀螺儀和加速度計的輸出模型，分別對兩輪自平衡智能車 進行靜態和動態條件下特性分析，進而得到各自包含誤差的數據輸出模型；
[0010] 步驟5,應用濾波算法對慣性傳感器陀螺儀和加速度計的數據進行最優估計，從而 實現對兩輪自平衡智能車的姿態解算，并完成對兩輪自平衡智能車的姿態控制。
[0011] 步驟1中所述動力學方程為：
[0013] 其中，L為兩輪自平衡智能車的擺桿質心到兩個同軸車輪軸線的距離；g為重力加 速度9. 8m/s2;Θ(t)為t時刻兩輪自平衡智能車與豎直方向的夾角即姿態傾角；x(t)為t 時刻由外界擾動所引起的兩輪自平衡智能車角加速度；a(t)為t時刻車輪運動產生的加速 度。
[0014] 當兩輪自平衡智能車處于自平衡狀態時，在姿態控制的作用下，姿態傾角變化范 圍較小（當保持自平衡狀態時變化范圍是比較小的，這是一種常用的對非線性方程或系統 進行線性化處理的方法）。因此在姿態傾角Θ(t) =0處通過泰勒級數展開進行線性化處 理，忽略高階項則有sinΘ(t) =Θ(t)，cosΘ(t) =1。與此同時，當智能車處于自平衡狀態 時，運行狀態應為靜止狀態或勻速直線運動狀態，則有智能車車輪運動產生的加速度a(t) =0。此時對應的動力學方程為：
[0016] 對上式進行拉氏變換得到兩輪自平衡智能車在自平衡狀態下s域內（s域是一種 公知的概念，應用于將微分方程轉換為代數方程的方法）姿態傾角Θ(s)與外界擾動所引 起的角加速度X(s)之
間的傳遞函數H(s):
[0018] 步驟2中，對兩輪自平衡智能車系統進行拉式變換后可知：系統在復平面內存在 兩個極點，
> 其中一個位于復平面的右側，此時系統的自由模態發散，系統不穩 定。為改善系統特性使自由模態收斂，對運動模型引入單位負反饋和串聯ro控制器后傳遞 函數H(s)為：
[0020] 其中，Kp、kd分別為ro控制器的比例系數和微分系數，κP、kd取值范圍為實數。對 上式使用勞斯判據進行分析，當滿足下列條件時，兩輪自平衡智能車姿態的自由模態收斂 并趨向于零從而達到穩定：
[0022] 步驟4中，慣性傳感器陀螺儀的輸出模型為：
[0023]cogyro=ωreal+εg+εn，
[0024] 其中為慣性傳感器陀螺儀輸出的角速度，ωraal為兩輪自平衡智能車真實的 角速度，％為慢時變漂移誤差，εn由溫度、摩擦力和不穩定力矩構成的隨機誤差；ε #口 εη的取值范圍為實數。
[0025] 慣性傳感器加速度計輸出模型為：
[0026] 9acce=Θreal+￡a,
[0027] 其中θ_表示慣性傳感器加速度計輸出的姿態傾角，Θraal為兩輪自平衡智能車 真實角度，ea為兩輪自平衡智能車運動時產生的動態加速度。慣性傳感器加速度計可以測 量運動和靜態的加速度。靜態加速度的一個特殊例子是重力加速度，當加速度傳感器靜止 時（也就是側面和垂直方向沒有加速度作用），那么作用在它上面的只有重力加速度。重力 (垂直）和加速度傳感器靈敏軸之間的夾角就是傾斜角，即可將加速度計用作傾角計。因為 角度由靈敏軸和重力矢量組成的垂直平面決定，傾斜可以從各種初始的慣性加速度傳感 器位置測得。
[0028] 步驟5中，采用如下公式對慣性傳感器陀螺儀和加速度計的輸出數據進行最優估 計：
[0032] Θ(k+Ι|k+Ι) =Θ(k+Ι|k)+Kg(k+1) (Θacce (k+Ι) -Θ(k+Ι|k)),
[0033] P(k+l|k+l) = (l-Kg(k+l))P(k+l|k)，
[0034] 其中：
[0035] Θ(k+l|k) :k+l時刻預估姿態傾角；
[0036] Θ(k|k) :k時刻的最優估計姿態傾角；
[0037] cogyra(k) :k時刻輸出角速度；
[0038] T:積分時間；
[0039] P(k+Ι|k) :Θ(k+Ι|k)對應的協方差；
[0040] P(k|k) :Θ(k|k)對應的協方差；
[0041] Kg(k+1) :k+l時刻卡爾曼增益；
[0042] Θa_(k+l):當前時刻姿態傾角；Θ(k+1 |k+l) :k+l時刻姿態傾角的最優估計；
[0043] Q:系統過程噪聲的協方差；
[0044] R:系統測量噪聲協方差；
[0045] 對以上公式進行遞歸運算即可得到姿態傾角的最優估計，從而實現對兩輪自平衡 智能車的姿態解算。
[0046] 有益效果：本發明適用于兩輪自平衡智能車的姿態解算，同時也適用于兩輪自平 衡機器人、四