任務內容

通過調整PID和LQR控制器以實現穩定懸停的多旋翼飛行器，運用在無論是在仿真中還是在實際系統中。

參考內容

LQR控制部分基礎參考內容：

LQR控制器

參考鏈接：

Linear Quadratic Regulator (LQR) With Python Code Example

設計方案

A：高度和偏航控制——PID控制器

對于PID的參數整形，可以參考如下三種方案（根據實機情況選擇其一，或將多種方案進行相互對比）

運用齊格勒-尼科爾斯法則（Ziegler-Nichols method）獲取初始估測值。

該方法是基于系統穩定性分析的PID整定方法．在設計過程中無需考慮任何特性要求，整定方法非常簡單，但控制效果卻比較理想．具體整定方法如下：

首先，假設只有比例控制，置$k_d=k_i=0$，然后增加比例系數一直到系統首次出現等幅振蕩（閉環系統的極點在ｊω軸上），此時獲取系統開始振蕩時的臨界增益$K_m$；

再將該比例系數根據下面的表格乘以對應的參數，這里乘以0.6

$$\begin{array}{cccc}\text{?調節器的類型?}& K_p& T_i& T_d\\ P& 0.5K_e& \infty & 0\\ PI& 0.45K_e& \frac{1}{1.2}{T}_e& 0\\ PID& 0.6K_e& 0.5T_e& 0.125T_e\end{array}$$

其他參數按照以下公式計算：

$$\begin{gathered} K_p=0.6?K_m \\ {K}_d=K_p?\frac{\pi }{4\omega } \\ {K}_i=K_p?\frac{\omega }{\pi } \end{gathered}$$

其中$K_p$為比例控制參數，$K_i$為積分控制參數，$K_d$為微分控制參數，$\omega$為振蕩時的頻率。

用上述法則確定PID控制器的參數，使系統的超調量在10%~60%之間，其平均值約為25%。

使用仿真獲取對應的增益值。

在真實系統中修改對應的增益值。

方法：緩慢增加 P 直至振蕩開始，然后慢慢加入少量D來抑制振蕩，然后慢慢添加少量的I來糾正任何穩態誤差。

B1：x，y位置控制——LQR控制器

對于多旋翼飛行器來說，線性運動方程表明了對于從位置坐標中獲取的x坐標$p_x$和y坐標$p_y$以及滾轉角$\beta$和俯仰角$\gamma$之間的完全解耦。

$$?\begin{array}{l}{\dot{p}}_x\\ {\ddot{p}}_x\\ \dot{\beta }\end{array}?=?\begin{array}{lll}0& 1& 0\\ 0& 0& g\\ 0& 0& 0\end{array}??\begin{array}{l}{p}_x\\ {\dot{p}}_x\\ \beta \end{array}?+?\begin{array}{l}0\\ 0\\ 1\end{array}?\left[{\omega }_y\right]$$

$$?\begin{array}{c}{\dot{p}}_y\\ {\ddot{p}}_y\\ \dot{\gamma }\end{array}?=?\begin{array}{ccc}0& 1& 0\\ 0& 0& ?g\\ 0& 0& 0\end{array}??\begin{array}{c}{p}_y\\ {\dot{p}}_y\\ \gamma \end{array}?+?\begin{array}{l}0\\ 0\\ 1\end{array}?\left[{\omega }_x\right]$$

因此，我們可以使用俯仰率${\omega }_y$來控制一個機體坐標 x 位置誤差，以及滾轉率${\omega }_x$來控制一個機體坐標 y 位置誤差。（內環 → 外環控制）

此任務的目標是分別為每個 x 和 y 設計一個 LQR 控制器。

Linear Quadratic Regulator，首字母縮略詞 LQR 代表線性二次調節器器。名稱本身指定此控制器設計方法適用的設置：

• 系統的動態是線性的，
• 要最小化的成本函數 (cost function) 是二次的，
• 系統化的控制器將狀態調節為零。

以下信息總結了合成一個無窮小界LQR控制器的步驟和方程。由此得到的控制器被稱為“線性狀態反饋控制器”，通常用矩陣“K”表示。狀態反饋矩陣K對系統的每個輸入有一行，對系統的每個狀態有一列。

• 連續時間和離散時間線性系統動力學分別記為

  $$\dot{x}=Ax+Bu,x_{k+1}=A_{\mathrm{D}}{x}_k+B_{\mathrm{D}}{u}_k$$

  為了將連續時間系統矩陣A和B轉換為離散時間系統矩陣$A_D$和$B_D$，使用MATLAB函數c2d，指定零階保持 (zero order hold) 作為離散化方法。

• 需要選擇Q和R成本函數 (cost function) 矩陣來實現飛行器的期望飛行性能。在無限時間跨度 (infinite-horizon) 的LQR代價函數為：

  $$J_{\infty }={\int }_0^{\infty }\left(x(t{)}^{?}Qx(t)+u(t{)}^{?}Ru(t)\right)dt$$

• Q是狀態成本矩陣，其行數和列數與狀態數相同，該矩陣權衡狀態向量中每個狀態的相對重要性，而R是輸入成本矩陣，行數與控制輸入的行數相同，列數與控制輸入的列數相同，該矩陣懲罰執行器的工作量。

  要選擇 Q 和 R 成本函數矩陣，應該考慮狀態向量的不同元素。例如，也許您希望懲罰10厘米的 x 位置偏差與偏航 5 度偏差的量相同。

• 使用 MATLAB 函數 care 和 dare 分別計算連續和離散時間的 Riccati 代數方程。可以參考MATLAB閱讀幫助文檔從而了解這些函數的計算。

• 連續時間線性時間不變（LTI）的無限時間跨度 LQR 設計方程系統是（直接取自控制系統I講義）：

  $$\begin{array}{rl}0& =?A^{?}{P}_{\infty }?P_{\infty }A+P_{\infty }B{R}^{?1}{B}^{?}{P}_{\infty }?Q\\ u(t)& =?K_{\infty }x(t)\\ K_{\infty }& =R^{?1}{B}^{?}{P}_{\infty }\end{array}$$

  其中：

  第一個方程是求解$P_{\infty }$的 Riccati 方程

  第二個是實現的控制率$u(t)$

  第三個是狀態反饋增益矩陣$K_{\infty }$

• 離散時間 LTI 系統的無限時間跨度 LQR 設計方程為：

  $$\begin{array}{rl}0& =?P_{\infty ,\mathrm{D}}+A_{\mathrm{D}}^{?}{P}_{\mathrm{D},\infty }{A}_{\mathrm{D}}?A_{\mathrm{D}}^{?}{P}_{\mathrm{D},\infty }{B}_{\mathrm{D}}{\left(B_{\mathrm{D}}^{?}{P}_{\mathrm{D},\infty }{B}_{\mathrm{D}}+R\right)}^{?1}{B}_{\mathrm{D}}^{?}{P}_{\mathrm{D},\infty }{A}_{\mathrm{D}}+Q\\ u_{\mathrm{D}}(k)& =?K_{\mathrm{D},\infty }{x}_{\mathrm{D}}(k)\\ K_{\mathrm{D},\infty }& ={\left(B_{\mathrm{D}}^{?}{P}_{\mathrm{D},\infty }{B}_{\mathrm{D}}+R\right)}^{?1}{B}_{\mathrm{D}}^{?}{P}_{\mathrm{D},\infty }{A}_{\mathrm{D}}\end{array}$$

  其中下標$(.{)}_D$表示適用于離散時間系統的變量

• care 和 dare 函數都可以返回狀態反饋增益矩陣，如果使用此矩陣，需要注意符號約定。

B2：x，y位置控制——PID控制器

如果您還希望為(x, y)位置實現一個PID控制器，這是相關的任務描述。

基于A方案中實現一個針對高度和偏航的PID控制器的進一步任務：設計、實現和調整一個PID控制器來控制 x 和 y 位置。

回想一下，在B方案中提供的線性化的運動方程，表明了 x 位置和俯仰角與 y 位置和滾轉角之間的完全解耦。

因此，我們可以使用俯仰率${\omega }_y$來控制機體坐標 x 位置誤差，而滾轉率${\omega }_x$來控制機體坐標 y 位置誤差。

由于俯仰（或滾轉）角度的動力學比x(或y)位置的動力學足夠快，因此我們也可以用一個嵌套的控制器合理地控制位置誤差。“外部控制”環取一個x位置誤差，并請求一個俯仰角β來糾正誤差，然后“內環”取這個請求的俯仰角β的誤差，并請求一個關于機體坐標y軸的角速率ωy來糾正誤差。

C：為x，y位置添加積分器

一旦您完成了先前B任務中(x, y)位置的LQR控制器的實現，請觀察在跟蹤(x, y)設定點時的穩態偏移量。嘗試調整飛行器中的電池的位置幾毫米，并再次觀察穩態偏移量。

觀察穩態偏移量，并在每個 x 和 y 位置控制器上添加一個積分器，以消除穩態偏移量。

算法邏輯

B1：x，y位置控制——LQR控制器

LQR 使用一種稱為動態規劃的技術。當飛行器在世界上移動時，在每個時間步長t處，使用一系列 for 循環（其中我們運行每個for循環N次）計算最佳控制輸入，這些循環輸出對應于最小總成本的 u（即控制輸入）。

• 初始化 LQR 函數：傳入 7 個參數。

  LQR(Actual State x, Desired State xf, Q, R, A, B, dt);

  x_error = Actual State x – Desired State xf

• 初始化時間步長 N

  通常將N設置為某個任意整數，如50。LQR 問題的解決方案以遞歸方式獲得，從最后一個時間步開始，并及時向后工作到第一個時間步。

  換句話說，for 循環（我們將在一秒鐘內到達這些循環）需要經過大量迭代（在本例中為 50 次）才能達到 P 的穩定值（我們將在下一步中介紹 P）。

• 初始化 P 為包含 N+1 個元素的空列表

  令 P[N]=Q

  循環i從N到1，分別用以下公式 (Riccati方程) 計算 P[i-1]

  P[i-1] = Q + ATP[i]A – (ATP[i]B)(R + BTP[i]B)-1(BTP[i]A)

• 初始化 K 和 u 分別為包含 N 個元素的空列表

  循環i從0到N-1，分別用以下公式計算狀態反饋增益矩陣 K[i]

  K[i] = -(R + BTP[i+1]B)-1BTP[i+1]A

  K 將保持最佳反饋增益值。這是一個重要的矩陣，當乘以狀態誤差$x(t)$時，將生成最佳控制輸入$u(t)$（請參閱下一步）。

• 循環i從0到N-1，分別用以下公式計算控制輸入

  u[i] = K[i] @ x_error

  我們對for循環進行N次迭代，直到我們得到最優控制輸入u的穩定值（例如u = [線速度v，角速度ω]）。我假設當N = 50時達到穩定值。

• 返回最佳控制量u_star

  u_star = u[N-1]

  最佳控制輸入u_star。這是我們在上面的上一步中計算的最后一個值。它位于u列表的最后。

  返回最佳控制輸入。這些輸入將被發送到我們的飛行器，以便它可以移動到新的狀態（即新的x位置，y位置和偏航角γ）。

代碼內容（部分）

這里以雙輪小車為例，分析LQR控制系統的代碼設計

import numpy as np# Description: Linear Quadratic Regulator example # (two-wheeled differential drive robot car)######################## DEFINE CONSTANTS ##################################### # Supress scientific notation when printing NumPy arrays np.set_printoptions(precision=3,suppress=True)# Optional Variables max_linear_velocity = 3.0 # meters per second max_angular_velocity = 1.5708 # radians per seconddef getB(yaw, deltat):"""Calculates and returns the B matrix3x2 matix ---> number of states x number of control inputsExpresses how the state of the system [x,y,yaw] changesfrom t-1 to t due to the control commands (i.e. control inputs).:param yaw: The yaw angle (rotation angle around the z axis) in radians :param deltat: The change in time from timestep t-1 to t in seconds:return: B matrix ---> 3x2 NumPy array"""B = np.array([ [np.cos(yaw)*deltat, 0],[np.sin(yaw)*deltat, 0],[0, deltat]])return Bdef state_space_model(A, state_t_minus_1, B, control_input_t_minus_1):"""Calculates the state at time t given the state at time t-1 andthe control inputs applied at time t-1:param: A The A state transition matrix3x3 NumPy Array:param: state_t_minus_1 The state at time t-1 3x1 NumPy Array given the state is [x,y,yaw angle] ---> [meters, meters, radians]:param: B The B state transition matrix3x2 NumPy Array:param: control_input_t_minus_1 Optimal control inputs at time t-1 2x1 NumPy Array given the control input vector is [linear velocity of the car, angular velocity of the car][meters per second, radians per second]:return: State estimate at time t3x1 NumPy Array given the state is [x,y,yaw angle] --->[meters, meters, radians]"""# These next 6 lines of code which place limits on the angular and linear # velocities of the robot car can be removed if you desire.control_input_t_minus_1[0] = np.clip(control_input_t_minus_1[0],-max_linear_velocity,max_linear_velocity)control_input_t_minus_1[1] = np.clip(control_input_t_minus_1[1],-max_angular_velocity,max_angular_velocity)state_estimate_t = (A @ state_t_minus_1) + (B @ control_input_t_minus_1) return state_estimate_tdef lqr(actual_state_x, desired_state_xf, Q, R, A, B, dt):"""Discrete-time linear quadratic regulator for a nonlinear system.Compute the optimal control inputs given a nonlinear system, cost matrices, current state, and a final state.Compute the control variables that minimize the cumulative cost.Solve for P using the dynamic programming method.:param actual_state_x: The current state of the system 3x1 NumPy Array given the state is [x,y,yaw angle] --->[meters, meters, radians]:param desired_state_xf: The desired state of the system3x1 NumPy Array given the state is [x,y,yaw angle] --->[meters, meters, radians] :param Q: The state cost matrix3x3 NumPy Array:param R: The input cost matrix2x2 NumPy Array:param dt: The size of the timestep in seconds -> float:return: u_star: Optimal action u for the current state 2x1 NumPy Array given the control input vector is[linear velocity of the car, angular velocity of the car][meters per second, radians per second]"""# We want the system to stabilize at desired_state_xf.x_error = actual_state_x - desired_state_xf# Solutions to discrete LQR problems are obtained using the dynamic # programming method.# The optimal solution is obtained recursively, starting at the last # timestep and working backwards.# You can play with this numberN = 50# Create a list of N + 1 elementsP = [None] * (N + 1)Qf = Q# LQR via Dynamic ProgrammingP[N] = Qf# For i = N, ..., 1for i in range(N, 0, -1):# Discrete-time Algebraic Riccati equation to calculate the optimal # state cost matrixP[i-1] = Q + A.T @ P[i] @ A - (A.T @ P[i] @ B) @ np.linalg.pinv(R + B.T @ P[i] @ B) @ (B.T @ P[i] @ A) # Create a list of N elementsK = [None] * Nu = [None] * N# For i = 0, ..., N - 1for i in range(N):# Calculate the optimal feedback gain KK[i] = -np.linalg.pinv(R + B.T @ P[i+1] @ B) @ B.T @ P[i+1] @ Au[i] = K[i] @ x_error# Optimal control input is u_staru_star = u[N-1]return u_stardef main():# Let the time interval be 1.0 secondsdt = 1.0# Actual state# Our robot starts out at the origin (x=0 meters, y=0 meters), and # the yaw angle is 0 radians. actual_state_x = np.array([0,0,0]) # Desired state [x,y,yaw angle]# [meters, meters, radians]desired_state_xf = np.array([2.000,2.000,np.pi/2]) # A matrix# 3x3 matrix -> number of states x number of states matrix# Expresses how the state of the system [x,y,yaw] changes # from t-1 to t when no control command is executed.# Typically a robot on wheels only drives when the wheels are told to turn.# For this case, A is the identity matrix.# Note: A is sometimes F in the literature.A = np.array([ [1.0, 0, 0],[ 0,1.0, 0],[ 0, 0, 1.0]])# R matrix# The control input cost matrix# Experiment with different R matrices# This matrix penalizes actuator effort (i.e. rotation of the # motors on the wheels that drive the linear velocity and angular velocity).# The R matrix has the same number of rows as the number of control# inputs and same number of columns as the number of # control inputs.# This matrix has positive values along the diagonal and 0s elsewhere.# We can target control inputs where we want low actuator effort # by making the corresponding value of R large. R = np.array([[0.01, 0], # Penalty for linear velocity effort[ 0, 0.01]]) # Penalty for angular velocity effort# Q matrix# The state cost matrix.# Experiment with different Q matrices.# Q helps us weigh the relative importance of each state in the # state vector (X, Y, YAW ANGLE). # Q is a square matrix that has the same number of rows as # there are states.# Q penalizes bad performance.# Q has positive values along the diagonal and zeros elsewhere.# Q enables us to target states where we want low error by making the # corresponding value of Q large.Q = np.array([[0.639, 0, 0], # Penalize X position error [0, 1.0, 0], # Penalize Y position error [0, 0, 1.0]]) # Penalize YAW ANGLE heading error # Launch the robot, and have it move to the desired goal destinationfor i in range(100):print(f'iteration = {i} seconds')print(f'Current State = {actual_state_x}')print(f'Desired State = {desired_state_xf}')state_error = actual_state_x - desired_state_xfstate_error_magnitude = np.linalg.norm(state_error) print(f'State Error Magnitude = {state_error_magnitude}')B = getB(actual_state_x[2], dt)# LQR returns the optimal control inputoptimal_control_input = lqr(actual_state_x, desired_state_xf, Q, R, A, B, dt) print(f'Control Input = {optimal_control_input}')# We apply the optimal control to the robot# so we can get a new actual (estimated) state.actual_state_x = state_space_model(A, actual_state_x, B, optimal_control_input) # Stop as soon as we reach the goal# Feel free to change this threshold value.if state_error_magnitude < 0.01:print("\nGoal Has Been Reached Successfully!")breakprint()# Entry point for the program main()

總結

以上是生活随笔為你收集整理的使用PID和LQR控制器进行多旋翼飞行器控制的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。