一種基于隱式Lyapunov函數的四旋翼飽和控制方法與流程

本發明涉及一種無人機的光滑輸入受限飛控控制方法，尤其是涉及一種基于隱式lyapunov函數的四旋翼飽和控制方法。

背景技術：

1、輸入飽和是一個非常典型的工程問題，任何物理對象其能量都是有限的，對于四旋翼而言，電機推力飽和是非常常見的。四旋翼的控制一般被分解為內外環(即內環和外環)，內環為姿態控制環，外環為位置控制環。這種分層控制的好處在于能夠將飛行器的高階系統進行降階處理，四旋翼可看作四階系統，而四階系統的調參要求極高，且控制響應較差。通過內外環結構，采用分層控制可將四階系統轉換為兩個二階系統。現有技術大多基于內外環控制框架設計，例如開源飛控如px4中，利用時標分離原理將整體飛控框架分成位置環控制和姿態環控制，并針對兩個環路分別設計了串級pid(比例-積分-微分)控制器。盡管在設計控制律時考慮到控制量的飽和問題，但處理方式僅僅是在控制律最后設計非光滑飽和函數：

2、

3、其中，x為控制律計算的期望控制量，um為給定的最小控制量限制幅值，um為給定的最大控制量限制幅值。

4、綜上，開源飛控px4在設計控制器時簡單的采用非光滑飽和函數對輸入進行限幅。這種非光滑飽和函數是非線性的，容易造成無人機系統響應曲線出現過沖或超調現象，對系統的控制性能影響較大。

技術實現思路

1、本發明提供了一種基于隱式lyapunov函數的四旋翼飽和控制方法，通過隱式lyapunov函數解決四旋翼飛行過程中輸入受限的問題，提出的控制方法能夠保證俯仰角、滾轉角、推力、控制力矩等關鍵指標被約束在設定范圍內。其技術方案如下所述：

2、一種基于隱式lyapunov函數的四旋翼飽和控制方法，包括以下步驟：

3、s1：設定飛行器初始條件，給定ned坐標系下飛行器的三維期望運動軌跡；

4、s2：通過給定輸入限制幅值，計算外環位置控制器縮放因子，以及求得隱式lyapunov函數變量vi的值，最后計算出期望升力、期望滾轉角和期望俯仰角；

5、s3：根據外環位置控制器計算得到的期望姿態角，引入姿態誤差，通過給定力矩幅值并設計內環姿態控制器，求得隱式lyapunov函數變量，最終得到三軸期望力矩；

6、s4：計算得到三軸力矩后，再結合外環位置控制器計算出的期望升力t進行控制分配求解各電機轉速。

7、進一步的，步驟s1中，設定在ned坐標系下，pd＝[xd,yd,zd]t為飛行器期望位置，為期望速度，為期望加速度；

8、其中x,y,z分別對應ned坐標系的北(n)-東(e)-地(d)三軸，xd,yd,zd分別對應三軸的期望坐標值，分別對應三軸的期望速度值，分別對應三軸的期望加速度值；設定位置誤差變量ep＝pd-p，速度誤差變量其中p＝[xe,ye,ze]t為四旋翼ned坐標系下的位置，v＝[vx,vy,vz]t為四旋翼ned坐標系下的速度，x,y,z分別對應ned坐標系的北(n)-東(e)-地(d)三軸。

9、進一步的，步驟s2中，計算出期望升力、期望滾轉角和期望俯仰角，包括以下步驟：

10、s11：給定輸入限制幅值；

11、s12：計算正定對稱矩陣pi和外環位置控制器縮放因子εi；

12、s13：引入隱式lyapunov函數，并采用數值算法求解隱式lyapunov函數變量的值，能夠保證飛行器控制律的光滑性，進而保證飛行時不會出現過沖或超調現象；

13、s14：根據隱式lyapunov函數vi的值計算控制輸入，即北-東-地的三軸加速度控制輸入；

14、s15：給定期望航向，計算出期望升力、期望滾轉角和期望俯仰角。

15、步驟s11中，給定輸入限制幅值，是根據設定的輸入幅值計算飛行器的最大滾轉角、俯仰角，或根據最大滾轉角、俯仰角反向尋找合適的輸入幅值，輸入幅值滿足下列不等式關系：

16、

17、

18、

19、其中φm為最大滾轉角，θm為最大俯仰角，加速度輸入限制幅值分別為ned坐標系下x,y,z軸最大加速度；為最大值比較函數，輸出為和中的最大值；g為地球重力加速度，為步驟s1中給定的z軸期望加速度。

20、步驟s12中，設定控制器增益其中和分別為pd控制器的比例項和微分項增益，利用控制器增益ki計算合適的正定對稱矩陣pi,pi滿足不等式關系：

21、1)

22、2)

23、獲得滿足上述條件的pi后，根據加速度輸入限制幅值計算縮放因子

24、

25、其中ki為控制器增益，為控制器增益的轉置，pi-1為正定對稱矩陣pi的逆，為加速度輸入限制幅值。i＝x,y,z分別代表三軸，且x,y,z分別對應ned坐標系的北(n)-東(e)-地(d)三軸。

26、步驟s13中，利用縮放因子εi構造等式其中vi為隱式lyapunov函數變量的值，pi為正定對稱矩陣；誤差狀態變量且為步驟s1中位置誤差變量ep第i個元素，為速度誤差變量ev第i個元素，i＝x,y,z分別代表x,y,z三軸，且x,y,z分別對應ned坐標系的北(n)-東(e)-地(d)三軸；

27、為縮放矩陣；

28、針對x,y,z軸分別采用newton-raphson法數值計算等式求得隱式lyapunov函數vi的值。qi(vi)為關于vi的函數。

29、步驟s14中，三軸加速度控制輸入u0：

30、

31、其中，利用lyapunov函數相關的對控制增益進行縮放，kx,ky,kz分別為x,y,z三軸控制器增益；ηx,ηy,ηz分別為x,y,z三軸的誤差狀態變量；εx,εy,εz分別為x,y,z三軸的縮放因子；vx,vy,vz分別為x,y,z三軸的隱式lyapunov函數的值；分別為x,y,z三軸的期望加速度；g為地球重力加速度。

32、步驟s15中，當給定期望航向ψc為任意常值或者根據步驟s1設定軌跡的期望速度采用函數計算獲得，能夠計算出期望升力t、期望滾轉角φc和期望俯仰角θc：

33、t＝m‖u0‖

34、

35、

36、其中m為飛行器質量，為三軸加速度控制輸入，i＝1,2,3為u0的第i個元素。

37、步驟s3中，包括以下步驟：

38、s21：將期望姿態角轉換為期望四元數，用于計算姿態誤差，

39、根據期望姿態角θc＝[φc,θc,ψc],通過如下姿態角與四元數轉換關系式獲取期望四元數qc的表達：

40、

41、其中為期望四元數qc的四個元素；φc,θc,ψc分別為期望滾轉角、期望俯仰角和期望航向角；

42、s22：通過期望四元數qc，結合飛行器姿態的四元數表達q和機體軸角速度ω，計算姿態誤差e；

43、引入姿態誤差e＝[eq,eω]t＝[q(q,qc),ω-ωc]t，其中q(q,qc)為期望四元數和飛行器姿態四元數的誤差，由如下等式計算得到：

44、

45、ω＝[p,q,r]t為飛行器機體軸角速度，ωc為期望機體軸角速度，由如下等式計算得到：

46、其中為期望四元數變化率；

47、s23：給定內環姿態控制器力矩幅值為輸入限制，給定三軸力矩幅值根據關系式：

48、

49、保證三軸力矩τj被約束在給定的范圍內；其中j為飛行器慣量陣，為機體軸期望角加速度，j＝φ,θ,ψ分別對應機體坐標系x,y,z三個軸；λ(j)為慣量陣j的特征值；

50、s24：所述內環姿態控制器用于獲得期望升力和力矩，模仿步驟s13、步驟s14，求得隱式lyapunov函數變量vj，計算姿態控制律，得到體軸系三軸期望力矩，即分別圍繞機體坐標系ox軸、oy軸、oz軸的力矩τ：

51、

52、其中kφ,kθ,kψ分別為機體坐標系x,y,z三軸控制增益；ηφ,ηθ,ηψ分別為x,y,z三軸的誤差狀態變量；εφ,εθ,εψ分別為x,y,z三軸的縮放因子；vφ,vθ,vψ分別為x,y,z三軸的隱式lyapunov函數的值；分別為x,y,z三軸的期望角加速度。

53、本發明提供的基于隱式lyapunov函數的四旋翼飽和控制方法，是一種基于隱式lypaunov函數的光滑輸入受限飛控算法，不同于開源飛控算法，常用的lyapunov函數大多為關于誤差v的二次型正定函數v＝xtpx，其中x為誤差變量，xt為誤差變量的轉置，p為正定矩陣，通常需要人為選擇，這種lyapunov函數為顯式lyapunov函數，因為其具備解析表達式。本發明中的隱式lyapunov函數可以看作一個變量，通過求解數值方程獲得。該控制方法的好處在于：

54、(1)分別針對內外環設計基于隱式lypaunov函數的控制器，在保證原始pd(比例-微分)控制響應特性的前提下，根據輸入的限制幅值自動調節控制增益大小，使得飛行器推力能夠保證維持在實際限制內。

55、(2)所設計的控制器不是簡單采用非光滑飽和函數，而是通過隱式lypaunov函數計算時變增益。由于隱式lyapunov函數通過求解數值方程獲得，保證了控制律的光滑性，進而保證了飛行時不會出現過沖或超調現象。

56、(3)通過對飛行器位置環設計合理的受限幅值，能夠保證飛行器飛行時姿態角不會超出某一設定值，對某些對姿態角有限制的場景下具有重要意義。