devbox
小小林熬夜学编程
这个屌丝很懒，什么也没留下！
关注作者
热门标签
热门文章
当前位置:   article > 正文

（2-3-1）位置控制算法：无人机运动控制系统（1）

作者：小小林熬夜学编程 | 2024-04-28 20:04:25

赞

踩

（2-3-1）位置控制算法：无人机运动控制系统（1）

2.3  无人机运动控制系统

本项目是一个基于Python的无人机运动控制算法库，提供了多个算法模块，包括基于鲁棒控制的Adaptive ISMC、经典的ISMC、线性系统的LQR等。该库通过模拟无人机的运动和姿态控制，在不同环境和故障条件下评估算法性能。同时，项目还包括了详细的文档和测试文件，方便用户理解和验证算法实现。本项目包含如下所示的功能模块：

  1. Copter Models：提供了无人机的非线性和线性动力学模型，用于模拟不同控制算法的性能。
  2. Control Algorithms：包括鲁棒控制算法Adaptive ISMC、经典的ISMC、线性系统的LQR等，用于实现无人机的运动和姿态控制。
  3. Fault Injection：具备故障注入功能，允许在模拟中引入不同类型的故障，评估算法对故障的鲁棒性。
  4. Simulation Environment：提供了模拟环境，支持在不同条件下评估算法性能，包括故障处理、控制分配等。
  5. Testing Infrastructure：包含详尽的测试文件和测试基础设施，确保算法的正确性和稳定性。

整体上，本项目旨在为用户提供一个全面的无人机运动控制算法库，支持算法开发、测试和验证。

实例2-8：无人机运动控制系统（源码路径：codes\2\locate\decmk-ftc-copter）

2.3.1  Fym介绍

Fym是一个通用的Python动力学模拟器库，它最初是为需要高度准确积分（例如Runge-Kutta-Fehlberg方法或简单的rk45）以及一组相互交互的组件的飞行模拟器而开发的。对于积分，Fym支持各种Scipy积分方法，此外还有自己的固定步长求解器，如rk4。此外，Fym具有一种新颖的结构，提供系统每个组件的模块化设计，与Simulink非常相似。

库Fym最早被用于开发准确的飞行模拟器，航空航天工程师可以与OpenAI Gym一起使用以研究强化学习，这也是库名称为Fym（Flight + Gym）的原因。尽管现在它是一个通用的动力学模拟器，但许多代码和思想是在OpenAI Gym中设计的。本项目是基于库Fym实现的，在学习之前先通过如下命令进行安装：

pip install fym

2.3.2  控制算法

在本项目的“agents”目录中包含了4个Python程序文件，实现了对多旋翼无人机的不同控制算法，这些控制器旨在应对多旋翼无人机在不同工况下的位置和姿态控制问题，提高系统的鲁棒性和性能。

1. 被动失效（LoE）故障模型

文件decmk/agents/utils.py定义了一个名为LoE的类，代表了一种被动失效（LoE）故障模型。该类包含两个方法：get_effectiveness用于获取失效模型的效果，get用于应用失效模型到输入信号上。在测试代码中，创建了一个LoE对象，设置了失效时间、失效位置和失效水平，然后演示了获取失效效果和应用失效模型的过程。

  1. import numpy as np
  2.  
  3. class LoE:
  4.     def __init__(self, time=0, index=0, level=1.0):
  5.         self.time = time
  6.         self.index = index
  7.         self.level = level
  8.  
  9.     def get(self, t, u):
  10.         effectiveness = self.get_effectiveness(t, np.ones(u.shape[0]))
  11.         return u * effectiveness.reshape(u.shape[0], 1)
  12.  
  13.     def get_effectiveness(self, t, effectiveness):
  14.         if t >= self.time:
  15.             effectiveness[self.index] = self.level
  16.         return effectiveness
  17.  
  18. if __name__ == "__main__":
  19.     t = 4
  20.     loe = LoE(time=3, index=0, level=0.5)
  21.     print(loe.get_effectiveness(t, np.array([1, 2, 3, 4])))
  22.     print(loe.get(t, np.array([1, 2, 3, 4])))

2. 基于线性和非线性的自适应容错滑模控制算法

文件decmk/agents/ISMC.py定义了两个类IntegralSMC_linear和IntegralSMC_nonlinear，分别实现了基于线性和非线性模型的自适应容错滑模控制算法方案。这些控制算法用于控制多旋翼无人机的位置移动，具备对执行器故障的适应性容错能力。其中包含位置跟踪的PD控制，滑模控制算法以及控制分配方案，以实现对飞行器的精确控制。文件decmk/agents/lqr.py的具体实现流程如下所示。

（1）定义了一个饱和函数sat，其功能是对输入变量s进行饱和处理，若s大于阈值eps，返回1；若s小于负阈值-eps，返回-1；否则返回s除以eps的结果。这样的函数通常用于限制变量在特定范围内波动。

  1. def sat(s, eps):
  2.     if s > eps:
  3.         return 1
  4.     elif s < -eps:
  5.         return -1
  6.     else:
  7.         return s/eps

（2）类IntegralSMC_linear 是基于自适应容错滑模控制方案的多旋翼无人机控制器，该类包含以下函数：

  1. __init__(self, J, m, g, d, ic, ref0)：初始化函数，设置控制器的初始参数和状态。
  2. deriv(self, obs, ref)：计算状态误差的导数，用于控制器设计。
  3. set_dot(self, obs, ref)：设置状态误差的导数。
  4. get_FM(self, obs, ref, p, K, Kc, PHI, t)：根据当前状态和参考状态计算控制力和滑模面，实现自适应容错滑模控制，返回控制力和滑模面。其中，参数obs表示当前状态，ref表示参考状态，p表示控制器参数，K表示PD控制器增益，Kc表示滑模面控制增益，PHI表示滑模面参数，t表示时间。
  1. class IntegralSMC_linear(BaseEnv):
  2.     '''
  3.     参考文献
  4.     Ban Wang, Youmin Zhang“一种自适应容错滑模控制分配方案用于多旋翼无人机受执行器故障影响”，IEEE工业电子学报，第65卷，第5期，2018年5月
  5.     '''
  6.  
  7.     def __init__(self, J, m, g, d, ic, ref0):
  8.         super().__init__()
  9.         self.ic = ic
  10.         self.ref0 = ref0
  11.         self.J, self.m, self.g, self.d = J, m, g, d
  12.         # x、y、z、roll、pitch、yaw的误差积分
  13.         self.P = BaseSystem(np.vstack((self.ic[0] - self.ref0[0],
  14.                                        self.ic[1] - self.ref0[0],
  15.                                        self.ic[2] - self.ref0[2],
  16.                                        self.ic[6] - self.ref0[6],
  17.                                        self.ic[7] - self.ref0[7],
  18.                                        self.ic[8] - self.ref0[8])))
  19.  
  20.     def deriv(self, obs, ref):
  21.         # 观测
  22.         obs = np.vstack((obs))
  23.         x, y, z = obs[0:3]
  24.         phi, theta, psi = obs[6:9]
  25.         # 参考
  26.         ref = np.vstack((ref))
  27.         x_r, y_r, z_r = ref[0:3]
  28.         phi_r, theta_r, psi_r = ref[6:9]
  29.         dP = np.vstack((x - x_r,
  30.                         y - y_r,
  31.                         z - z_r,
  32.                         phi - phi_r,
  33.                         theta - theta_r,
  34.                         psi - psi_r))
  35.  
  36.         return dP
  37.  
  38.     def set_dot(self, obs, ref):
  39.         dot = self.deriv(obs, ref)
  40.         self.P.dot = dot
  41.  
  42.     def get_FM(self, obs, ref, p, K, Kc, PHI, t):
  43.         p = np.vstack((p))
  44.         px, py, pz, pphi, ptheta, ppsi = p
  45.         K1, K2, K3, K4 = K
  46.         k11, k12 = K1
  47.         k21, k22 = K2
  48.         k31, k32 = K3
  49.         k41, k42 = K4
  50.         kc1, kc2, kc3, kc4 = Kc
  51.         PHI1, PHI2, PHI3, PHI4 = PHI
  52.         # 模型
  53.         J = self.J
  54.         Ixx = J[0, 0]
  55.         Iyy = J[1, 1]
  56.         Izz = J[2, 2]
  57.         m, g, d = self.m, self.g, self.d
  58.         # 观测
  59.         obs = np.vstack((obs))
  60.         x, y, z, xd, yd, zd = obs[0:6]
  61.         phi, theta, psi, phid, thetad, psid = obs[6:]
  62.         # 参考
  63.         ref = np.vstack((ref))
  64.         x_r, y_r, z_r, xd_r, yd_r, zd_r = ref[0:6]
  65.         phi_r, theta_r, psi_r, phid_r, thetad_r, psid_r = ref[6:]
  66.         zdd_r = 0
  67.         phidd_r = 0
  68.         thetadd_r = 0
  69.         psidd_r = 0
  70.         # 初始条件
  71.         z0, z0d = self.ic[2], self.ic[5]
  72.         phi0, theta0, psi0, phi0d, theta0d, psi0d = self.ic[6:]
  73.         z0_r, z0d_r = self.ref0[2], self.ref0[5]
  74.         phi0_r, theta0_r, psi0_r, phi0d_r, theta0d_r, psi0d_r = self.ref0[6:]
  75.         # 位置跟踪的PD控制（获取phi_ref、theta_ref）
  76.         e_x = x - x_r
  77.         e_xd = xd - xd_r
  78.         e_y = y - y_r
  79.         e_yd = yd - yd_r
  80.         kp1, kd1, ki1 = np.array([0.5, 0.3, 0.2])
  81.         kp2, kd2, ki2 = np.array([0.5, 0.3, 0.2])
  82.         phi_r = -(kp1*e_y + kd1*e_yd + ki1*py)
  83.         theta_r = kp2*e_x + kd2*e_xd + ki2*px
  84.         # 误差定义
  85.         e_z = z - z_r
  86.         e_zd = zd - zd_r
  87.         e_phi = phi - phi_r
  88.         e_phid = phid - phid_r
  89.         e_theta = theta - theta_r
  90.         e_thetad = thetad - thetad_r
  91.         e_psi = psi - psi_r
  92.         e_psid = psid - psid_r
  93.         # h**(-1)函数定义
  94.         h1 = -m/cos(phi)/cos(theta)
  95.         h2 = Ixx/d
  96.         h3 = Iyy/d
  97.         h4 = Izz
  98.         # 滑模面
  99.         s1 = e_zd + k12*e_z + k11*pz - k12*(z0-z0_r) - (z0d-z0d_r)
  100.         s2 = e_phid + k22*e_phi + k21*pphi - k22*(phi0-phi0_r) - (phi0d-phi0d_r)
  101.         s3 = e_thetad + k32*e_theta + k31*ptheta - k32*(theta0-theta0_r) - (theta0d-theta0d_r)
  102.         s4 = e_psid + k42*e_psi + k41*ppsi - k42*(psi0-psi0_r) - (psi0d-psi0d_r)
  103.         # 获取FM
  104.         F = h1*(zdd_r - k12*e_zd - k11*e_z - g) - h1*kc1*sat(s1, PHI1)
  105.         M1 = h2*(phidd_r - k22*e_phid - k21*e_phi - (Iyy-Izz)/Ixx*thetad*psid) \
  106.             - h2*kc2*sat(s2, PHI2)
  107.         M2 = h3*(thetadd_r - k32*e_thetad - k31*e_theta
  108.                  - (Izz-Ixx)/Iyy*phid*psid) - h3*kc3*sat(s3, PHI3)
  109.         M3 = h4*(psidd_r - k42*e_psid - k41*e_psi - (Ixx-Iyy)/Izz*phid*thetad) \
  110.             - h4*kc4*sat(s4, PHI4)
  111.  
  112.         action = np.vstack((F, M1, M2, M3))
  113.         sliding = np.vstack((s1, s2, s3, s4))
  114.         return action, sliding

（3）类IntegralSMC_nonlinear 是一个基于自适应容错滑模控制（Adaptive Fault-Tolerant Sliding Mode Control）方案的多旋翼无人机控制环境，该类实现了一个多旋翼无人机的自适应容错滑模控制器，通过对状态变量的控制力和力矩进行计算，实现对无人机运动的控制。类IntegralSMC_nonlinear 中的主要函数及其功能如下所示。

  1. __init__(self, J, m, g, d, ic, ref0)：初始化函数，设置控制环境的初始条件和参考值。
  2. deriv(self, obs, ref)：计算状态变量的导数，用于控制器的微分部分。
  3. set_dot(self, obs, ref)：设置状态变量的导数，用于更新控制器的微分项。
  4. get_FM(self, obs, ref, p, t)：根据当前状态、参考状态和控制参数，计算多旋翼无人机的控制力和力矩。其中，obs 是当前状态，ref 是参考状态，p 是控制参数，t 是时间。
  1. class IntegralSMC_nonlinear(BaseEnv):
  2.     '''
  3.     参考文献
  4.     Ban Wang, Youmin Zhang，“一种自适应容错滑模控制分配方案用于多旋翼无人机受执行器故障影响”，IEEE工业电子学报，第65卷，第5期，2018年5月
  5.     '''
  6.  
  7.     def __init__(self, J, m, g, d, ic, ref0):
  8.         super().__init__()
  9.         self.ic_ = np.vstack((ic[0:6], np.vstack(quat2angle(ic[6:10])[::-1]), ic[10:]))
  10.         self.ref0_ = np.vstack((ref0[0:6], np.vstack(quat2angle(ref0[6:10])[::-1]), ref0[10:]))
  11.         self.J, self.m, self.g, self.d = J, m, g, d
  12.         # x、y、z、roll、pitch、yaw的误差积分
  13.         self.P = BaseSystem(np.vstack((self.ic_[0] - self.ref0_[0],
  14.                                        self.ic_[1] - self.ref0_[1],
  15.                                        self.ic_[2] - self.ref0_[2],
  16.                                        self.ic_[6] - self.ref0_[6],
  17.                                        self.ic_[7] - self.ref0_[7],
  18.                                        self.ic_[8] - self.ref0_[8])))
  19.  
  20.     def deriv(self, obs, ref):
  21.         # 观测
  22.         obs = np.vstack((obs))
  23.         obs_ = np.vstack((obs[0:6], np.vstack(quat2angle(obs[6:10])[::-1]), obs[10:]))
  24.         x, y, z = obs_[0:3]
  25.         phi, theta, psi = obs_[6:9]
  26.         # 参考
  27.         ref = np.vstack((ref))
  28.         ref_ = np.vstack((ref[0:6], np.vstack(quat2angle(ref[6:10])[::-1]), ref[10:]))
  29.         x_r, y_r, z_r = ref_[0:3]
  30.         phi_r, theta_r, psi_r = ref_[6:9]
  31.         dP = np.vstack((x - x_r,
  32.                         y - y_r,
  33.                         z - z_r,
  34.                         phi - phi_r,
  35.                         theta - theta_r,
  36.                         psi - psi_r))
  37.  
  38.         return dP
  39.  
  40.     def set_dot(self, obs, ref):
  41.         dot = self.deriv(obs, ref)
  42.         self.P.dot = dot
  43.  
  44.     def get_FM(self, obs, ref, p, t):
  45.         K = np.array([[25, 15],
  46.                       [40, 20],
  47.                       [40, 20],
  48.                       [1, 1]])
  49.         Kc = np.vstack((15, 15, 15, 10))
  50.         PHI = np.vstack((0.8, 0.1, 0.1, 1))
  51.         p = np.vstack((p))
  52.         px, py, pz, pphi, ptheta, ppsi = p
  53.         K1, K2, K3, K4 = K
  54.         k11, k12 = K1
  55.         k21, k22 = K2
  56.         k31, k32 = K3
  57.         k41, k42 = K4
  58.         kc1, kc2, kc3, kc4 = Kc
  59.         PHI1, PHI2, PHI3, PHI4 = PHI
  60.         # 模型
  61.         J = self.J
  62.         Ixx = J[0, 0]
  63.         Iyy = J[1, 1]
  64.         Izz = J[2, 2]
  65.         m, g, d = self.m, self.g, self.d
  66.         # 观测
  67.         obs = np.vstack((obs))
  68.         obs_ = np.vstack((obs[0:6], np.vstack(quat2angle(obs[6:10])[::-1]), obs[10:]))
  69.         x, y, z, xd, yd, zd = obs_[0:6]
  70.         phi, theta, psi, phid, thetad, psid = obs_[6:]
  71.         # 参考
  72.         ref_ = np.vstack((ref[0:6], np.vstack(quat2angle(ref[6:10])[::-1]), ref[10:]))
  73.         x_r, y_r, z_r, xd_r, yd_r, zd_r = ref_[0:6]
  74.         phi_r, theta_r, psi_r, phid_r, thetad_r, psid_r = ref_[6:]
  75.         zdd_r = 0
  76.         phidd_r = 0
  77.         thetadd_r = 0
  78.         psidd_r = 0
  79.         # 初始条件
  80.         z0, z0d = self.ic_[2], self.ic_[5]
  81.         phi0, theta0, psi0, phi0d, theta0d, psi0d = self.ic_[6:]
  82.         z0_r, z0d_r = self.ref0_[2], self.ref0_[5]
  83.         phi0_r, theta0_r, psi0_r, phi0d_r, theta0d_r, psi0d_r = self.ref0_[6:]
  84.         # 位置跟踪的PD控制（获取phi_ref、theta_ref）
  85.         e_x = x - x_r
  86.         e_xd = xd - xd_r
  87.         e_y = y - y_r
  88.         e_yd = yd - yd_r
  89.         kp1, kd1, ki1 = np.array([0.25, 0.11, 0.045])
  90.         kp2, kd2, ki2 = np.array([0.25, 0.13, 0.03])
  91.         phi_r = -(kp1*e_y + kd1*e_yd + ki1*py)
  92.         theta_r = kp2*e_x + kd2*e_xd + ki2*px
  93.         # 误差定义
  94.         e_z = z - z_r
  95.         e_zd = zd - zd_r
  96.         e_phi = phi - phi_r
  97.         e_phid = phid - phid_r
  98.         e_theta = theta - theta_r
  99.         e_thetad = thetad - thetad_r
  100.         e_psi = psi - psi_r
  101.         e_psid = psid - psid_r
  102.         # h**(-1)函数定义
  103.         h1 = -m/cos(phi)/cos(theta)
  104.         h2 = Ixx/d
  105.         h3 = Iyy/d
  106.         h4 = Izz
  107.         # 滑模面
  108.         s1 = e_zd + k12*e_z + k11*pz - k12*(z0-z0_r) - (z0d-z0d_r)
  109.         s2 = e_phid + k22*e_phi + k21*pphi - k22*(phi0-phi0_r) - (phi0d-phi0d_r)
  110.         s3 = e_thetad + k32*e_theta + k31*ptheta - k32*(theta0-theta0_r) - (theta0d-theta0d_r)
  111.         s4 = e_psid + k42*e_psi + k41*ppsi - k42*(psi0-psi0_r) - (psi0d-psi0d_r)
  112.         # 获取FM
  113.         F = h1*(zdd_r - k12*e_zd - k11*e_z - g) - h1*kc1*sat(s1, PHI1)
  114.         M1 = h2*(phidd_r - k22*e_phid - k21*e_phi - (Iyy-Izz)/Ixx*thetad*psid) - h2*kc2*sat(s2, PHI2)
  115.         M2 = h3*(thetadd_r - k32*e_thetad - k31*e_theta - (Izz-Ixx)/Iyy*phid*psid) - h3*kc3*sat(s3, PHI3)
  116.         M3 = h4*(psidd_r - k42*e_psid - k41*e_psi - (Ixx-Iyy)/Izz*phid*thetad) - h4*kc4*sat(s4, PHI4)
  117.  
  118.         action = np.vstack((F, M1, M2, M3))
  119.  
  120.         return action

3. 最优控制算法

文件decmk/agents/lqr.py定义了一个线性二次型调节器（LQR）控制器和相应的控制类LQRController。LQR控制器通过计算系统状态和参考状态之间的差异，并应用调整力矩，实现对多旋翼无人机的姿态和位置控制。控制器中包括状态转换函数、初始化函数以及计算控制力和力矩的函数。LQR（线性二次型调节器）是最优控制中的一种经典算法。它基于状态空间模型，通过最小化状态与控制输入的二次代价函数来设计控制器，从而实现系统的最优控制。LQR在工程控制领域广泛应用，尤其在线性系统和近似线性系统的控制中表现出色。

  1. def LQR(A: np.array, B: np.array, Q: np.array, R: np.array, with_eigs=False) \
  2.         -> np.array:
  3.     P = lin.solve_continuous_are(A, B, Q, R)
  4.     if np.size(R) == 1:
  5.         K = (np.transpose(B).dot(P)) / R
  6.     else:
  7.         K = np.linalg.inv(R).dot((np.transpose(B).dot(P)))
  8.  
  9.     eig_vals, eig_vecs = np.linalg.eig(A - B.dot(K))
  10.  
  11.     if with_eigs:
  12.         return K, P, eig_vals, eig_vecs
  13.     else:
  14.         return K, P
  15.  
  16. class LQRController:
  17.     def __init__(self, Jinv, m, g,
  18.                  Q=np.diag([1, 1, 1, 0, 0, 0, 1, 1, 1, 0, 0, 0]),
  19.                  R=np.diag([1, 1, 1, 1]),
  20.                  ):
  21.         self.Jinv = Jinv
  22.         self.m, self.g = m, g
  23.         self.trim_forces = np.vstack([self.m * self.g, 0, 0, 0])
  24.         '''
  25.         linearized condition near trim pt
  26.         state variables with {pos, vel, Euler angle, omega}
  27.         '''
  28.         A = np.array([[0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0],
  29.                       [0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0],
  30.                       [0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0],
  31.                       [0, 0, 0, 0, 0, 0, 0, -g, 0, 0, 0, 0],
  32.                       [0, 0, 0, 0, 0, 0, g, 0, 0, 0, 0, 0],
  33.                       [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
  34.                       [0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0],
  35.                       [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0],
  36.                       [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1],
  37.                       [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
  38.                       [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
  39.                       [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]])
  40.         B = np.array([[0, 0, 0, 0],
  41.                       [0, 0, 0, 0],
  42.                       [0, 0, 0, 0],
  43.                       [0, 0, 0, 0],
  44.                       [0, 0, 0, 0],
  45.                       [-1/m, 0, 0, 0],
  46.                       [0, 0, 0, 0],
  47.                       [0, 0, 0, 0],
  48.                       [0, 0, 0, 0],
  49.                       [0, Jinv[0, 0], 0, 0],
  50.                       [0, 0, Jinv[1, 1], 0],
  51.                       [0, 0, 0, Jinv[2, 2]]])
  52.  
  53.         self.K, *_ = LQR(A, B, Q, R)
  54.  
  55.     def transform(self, y):
  56.         # input comes in a form {pos, vel, quat, omega}
  57.         if len(y) == 13:
  58.             return np.vstack((y[0:6],
  59.                               np.vstack(quat2angle(y[6:10])[::-1]), y[10:]))
  60.  
  61.     def get_FM(self, obs, ref):
  62.         x = self.transform(obs)
  63.         x_ref = self.transform(ref)
  64.         forces = -self.K.dot(x - x_ref) + self.trim_forces
  65.         return forces

4. 自适应滑模控制（Adaptive Sliding Mode Control）算法

文件decmk/agents/AdaptiveISMC.py定义了两个多旋翼无人机控制器：AdaptiveISMC_linear 和 AdaptiveISMC_nonlinear。这些控制器基于自适应滑模控制（Adaptive Sliding Mode Control）实现，用于处理多旋翼无人机在存在故障情况下的姿态和位置控制。其中，AdaptiveISMC_linear采用线性模型，而AdaptiveISMC_nonlinear采用非线性模型。这些控制器通过引入滑动模式来实现鲁棒性，并通过自适应调节参数以适应系统变化。控制器的功能包括状态变量的微分方程、控制输入的计算以及反馈控制策略的实现。

文件decmk/agents/AdaptiveISMC.py的具体实现流程如下所示。

（1）类 AdaptiveISMC_linear 是一个多旋翼无人机控制器，基于自适应滑模控制方法，旨在处理多旋翼无人机在存在执行器故障的情况下的位置和姿态控制。该类包含了如下所示的功能函数：

  1. __init__(self, J, m, g, d, ic, ref0)：类的初始化函数，设置控制器的初始参数和系统状态。
  2. deriv(self, obs, ref, sliding)：计算多旋翼无人机系统的状态变量导数，用于模拟系统的动态行为。函数接受当前状态、参考状态和滑动表面作为输入，返回状态导数。
  3. set_dot(self, obs, ref, sliding)：根据 deriv 函数计算的状态导数，更新控制器内部的状态变量。
  4. get_FM(self, obs, ref, p, gamma, K, Kc, PHI, t)：根据当前状态、参考状态以及控制器的参数和状态，计算多旋翼无人机的控制输入和滑动表面。返回计算得到的控制输入和滑动表面。
  1. class AdaptiveISMC_linear(BaseEnv):
  2.  
  3.     def __init__(self, J, m, g, d, ic, ref0):
  4.         super().__init__()
  5.         self.ic = ic
  6.         self.ref0 = ref0
  7.         self.J, self.m, self.g, self.d = J, m, g, d
  8.         # error integral of x, y, z, roll, pitch, yaw
  9.         self.P = BaseSystem(np.vstack((self.ic[0] - self.ref0[0],
  10.                                        self.ic[1] - self.ref0[1],
  11.                                        self.ic[2] - self.ref0[2],
  12.                                        self.ic[6] - self.ref0[6],
  13.                                        self.ic[7] - self.ref0[7],
  14.                                        self.ic[8] - self.ref0[8])))
  15.         # parameter update, estimation
  16.         self.gamma = BaseSystem(np.vstack((-m/cos(ic[6])/cos(ic[7]),
  17.                                            J[0, 0]/d,
  18.                                            J[1, 1]/d,
  19.                                            J[2, 2])))
  20.  
  21.     def deriv(self, obs, ref, sliding):
  22.         J, m, g = self.J, self.m, self.g
  23.         Ixx = J[0, 0]
  24.         Iyy = J[1, 1]
  25.         Izz = J[2, 2]
  26.         K = self.K
  27.         Kc = self.Kc
  28.         PHI = self.PHI
  29.         K1, K2, K3, K4 = K
  30.         k11, k12 = K1
  31.         k21, k22 = K2
  32.         k31, k32 = K3
  33.         k41, k42 = K4
  34.         kc1, kc2, kc3, kc4 = Kc
  35.         PHI1, PHI2, PHI3, PHI4 = PHI
  36.         # observation
  37.         obs = np.vstack((obs))
  38.         x, y, z = obs[0:3]
  39.         zd = obs[5]
  40.         phi, theta, psi, phid, thetad, psid = obs[6:]
  41.         # reference
  42.         ref = np.vstack((ref))
  43.         x_r, y_r, z_r = ref[0:3]
  44.         zd_r = ref[5]
  45.         phi_r, theta_r, psi_r, phid_r, thetad_r, psid_r = ref[6:]
  46.         # reference ddot term(z, phi, theta, psi)
  47.         F_r = m*g
  48.         M1_r = 0
  49.         M2_r = 0
  50.         M3_r = 0
  51.         zdd_r = g - cos(phi_r)*cos(theta_r)*F_r/m
  52.         phidd_r = (Iyy-Izz)/Ixx*thetad_r*psid_r + 1/Ixx*M1_r
  53.         thetadd_r = (Izz-Ixx)/Iyy*phid_r*psid_r + 1/Iyy*M2_r
  54.         psidd_r = (Ixx-Iyy)/Izz*phid_r*thetad_r + 1/Izz*M3_r
  55.         # error
  56.         e_x = x - x_r
  57.         e_y = y - y_r
  58.         e_z = z - z_r
  59.         e_zd = zd - zd_r
  60.         e_phi = phi - phi_r
  61.         e_phid = phid - phid_r
  62.         e_theta = theta - theta_r
  63.         e_thetad = thetad - thetad_r
  64.         e_psi = psi - psi_r
  65.         e_psid = psid - psid_r
  66.         # sliding surface
  67.         s1, s2, s3, s4 = sliding
  68.         # algebraic distance btw current state and boundary layer
  69.         dels1 = s1 - PHI1*sat(s1, PHI1)
  70.         dels2 = s2 - PHI2*sat(s2, PHI2)
  71.         dels3 = s3 - PHI3*sat(s3, PHI3)
  72.         dels4 = s4 - PHI4*sat(s4, PHI4)
  73.         # derivs
  74.         dP = np.vstack((e_x, e_y, e_z, e_phi, e_theta, e_psi))
  75.  
  76.         dgamma1 = -(- zdd_r + k12*e_zd + k11*e_z + g + kc1*sat(s1, PHI1))*dels1
  77.         # dgamma1 should have negative sign to make Lyapunov like func > 0
  78.         dgamma2 = (- phidd_r + k22*e_phid + k21*e_phi
  79.                    + (Iyy-Izz)/Ixx*thetad_r*psid_r + kc2*sat(s2, PHI2))*dels2
  80.         dgamma3 = (- thetadd_r + k32*e_thetad + k31*e_theta
  81.                    + (Izz-Ixx)/Iyy*phid_r*psid_r + kc3*sat(s3, PHI3))*dels3
  82.         dgamma4 = (- psidd_r + k42*e_psid + k41*e_psi
  83.                    + (Ixx-Iyy)/Izz*phid_r*thetad_r + kc4*sat(s4, PHI4))*dels4
  84.         dgamma = np.vstack((dgamma1, dgamma2, dgamma3, dgamma4))
  85.  
  86.         return dP, dgamma
  87.  
  88.     def set_dot(self, obs, ref, sliding):
  89.         dots = self.deriv(obs, ref, sliding)
  90.         self.P.dot, self.gamma.dot = dots
  91.  
  92.     def get_FM(self, obs, ref, p, gamma, K, Kc, PHI, t):
  93.         self.K = K
  94.         self.Kc = Kc
  95.         self.PHI = PHI
  96.         p = np.vstack((p))
  97.         px, py, pz, pphi, ptheta, ppsi = p
  98.         gamma = np.vstack((gamma))
  99.         gamma1, gamma2, gamma3, gamma4 = gamma
  100.         K1, K2, K3, K4 = K
  101.         k11, k12 = K1
  102.         k21, k22 = K2
  103.         k31, k32 = K3
  104.         k41, k42 = K4
  105.         kc1, kc2, kc3, kc4 = Kc
  106.         PHI1, PHI2, PHI3, PHI4 = PHI
  107.         # model
  108.         J, g, m = self.J, self.g, self.m
  109.         Ixx = J[0, 0]
  110.         Iyy = J[1, 1]
  111.         Izz = J[2, 2]
  112.         # observation
  113.         obs = np.vstack((obs))
  114.         x, y, z, xd, yd, zd = obs[0:6]
  115.         phi, theta, psi, phid, thetad, psid = obs[6:]
  116.         # reference
  117.         x_r, y_r, z_r, xd_r, yd_r, zd_r = ref[0:6]
  118.         phi_r, theta_r, psi_r, phid_r, thetad_r, psid_r = ref[6:]
  119.         # reference ddot term(z, phi, theta, psi)
  120.         F_r = m*g
  121.         M1_r = 0
  122.         M2_r = 0
  123.         M3_r = 0
  124.         zdd_r = g - cos(phi_r)*cos(theta_r)*F_r/m
  125.         phidd_r = (Iyy-Izz)/Ixx*thetad_r*psid_r + 1/Ixx*M1_r
  126.         thetadd_r = (Izz-Ixx)/Iyy*phid_r*psid_r + 1/Iyy*M2_r
  127.         psidd_r = (Ixx-Iyy)/Izz*phid_r*thetad_r + 1/Izz*M3_r
  128.         # initial condition
  129.         z0, z0d = self.ic[2], self.ic[5]
  130.         phi0, theta0, psi0, phi0d, theta0d, psi0d = self.ic[6:]
  131.         z0_r, z0d_r = self.ref0[2], self.ref0[5]
  132.         phi0_r, theta0_r, psi0_r, phi0d_r, theta0d_r, psi0d_r = self.ref0[6:]
  133.         # PD control for position tracking (get phi_ref, theta_ref)
  134.         e_x = x - x_r
  135.         e_xd = xd - xd_r
  136.         e_y = y - y_r
  137.         e_yd = yd - yd_r
  138.         kp1, kd1, ki1 = np.array([0.3, 0.2, 0.1])
  139.         kp2, kd2, ki2 = np.array([0.3, 0.2, 0.1])
  140.         phi_r = -(kp1*e_y + kd1*e_yd + ki1*py)
  141.         theta_r = kp2*e_x + kd2*e_xd + ki2*px
  142.         # error definition
  143.         e_z = z - z_r
  144.         e_zd = zd - zd_r
  145.         e_phi = phi - phi_r
  146.         e_phid = phid - phid_r
  147.         e_theta = theta - theta_r
  148.         e_thetad = thetad - thetad_r
  149.         e_psi = psi - psi_r
  150.         e_psid = psid - psid_r
  151.         # sliding surface
  152.         s1 = e_zd + k12*e_z + k11*pz - k12*(z0-z0_r) - (z0d-z0d_r)
  153.         s2 = e_phid + k22*e_phi + k21*pphi - k22*(phi0-phi0_r) - (phi0d-phi0d_r)
  154.         s3 = e_thetad + k32*e_theta + k31*ptheta - k32*(theta0-theta0_r) \
  155.             - (theta0d-theta0d_r)
  156.         s4 = e_psid + k42*e_psi + k41*ppsi - k42*(psi0-psi0_r) - (psi0d-psi0d_r)
  157.         # get FM
  158.         F = gamma1*(zdd_r - k12*e_zd - k11*e_z - g) - gamma1*kc1*sat(s1, PHI1)
  159.         M1 = gamma2*(phidd_r - k22*e_phid - k21*e_phi - (Iyy-Izz)/Ixx*thetad*psid) \
  160.             - gamma2*kc2*sat(s2, PHI2)
  161.         M2 = gamma3*(thetadd_r - k32*e_thetad - k31*e_theta - (Izz-Ixx)/Iyy*phid*psid) \
  162.             - gamma3*kc3*sat(s3, PHI3)
  163.         M3 = gamma4*(psidd_r - k42*e_psid - k41*e_psi - (Ixx-Iyy)/Izz*phid*thetad) \
  164.             - gamma4*kc4*sat(s4, PHI4)
  165.  
  166.         action = np.vstack((F, M1, M2, M3))
  167.         sliding = np.vstack((s1, s2, s3, s4))
  168.  
  169.         return action, sliding

（2）类 AdaptiveISMC_nonlinear 也是一个多旋翼无人机控制器，同样基于自适应滑模控制方法，专门设计用于处理多旋翼无人机在存在执行器故障的情况下的位置和姿态控制。在类 AdaptiveISMC_nonlinear包含如下所示的内置函数。

  1. __init__(self, J, m, g, d, ic, ref0)：类的初始化函数，设置控制器的初始参数和系统状态。
  2. deriv(self, obs, ref, sliding)：计算多旋翼无人机系统的状态变量导数，用于模拟系统的动态行为。函数接受当前状态、参考状态和滑动表面作为输入，返回状态导数。
  3. set_dot(self, obs, ref, sliding)：根据 deriv 函数计算的状态导数，更新控制器内部的状态变量。特别地，在这个类中，还包括对控制器状态的一些修正。
  4. get_FM(self, obs, ref, p, gamma, t)：根据当前状态、参考状态以及控制器的参数和状态，计算多旋翼无人机的控制输入和滑动表面。返回计算得到的控制输入和滑动表面。
  1. class AdaptiveISMC_nonlinear(BaseEnv):
  2.  
  3.     def __init__(self, J, m, g, d, ic, ref0):
  4.         super().__init__()
  5.         self.ic_ = np.vstack((ic[0:6],
  6.                               np.vstack(quat2angle(ic[6:10])[::-1]),
  7.                               ic[10:]))
  8.         self.ref0_ = np.vstack((ref0[0:6],
  9.                                 np.vstack(quat2angle(ref0[6:10])[::-1]),
  10.                                 ref0[10:]))
  11.         self.J, self.m, self.g, self.d = J, m, g, d
  12.         # error integral of x, y, z, roll, pitch, yaw
  13.         self.P = BaseSystem(np.vstack((self.ic_[0] - self.ref0_[0],
  14.                                        self.ic_[1] - self.ref0_[1],
  15.                                        self.ic_[2] - self.ref0_[2],
  16.                                        self.ic_[6] - self.ref0_[6],
  17.                                        self.ic_[7] - self.ref0_[7],
  18.                                        self.ic_[8] - self.ref0_[8])))
  19.         # parameter update, estimation
  20.         self.gamma = BaseSystem(np.vstack((-m/cos(self.ic_[6])/cos(self.ic_[7]),
  21.                                            J[0, 0]/d,
  22.                                            J[1, 1]/d,
  23.                                            J[2, 2])))
  24.  
  25.     def deriv(self, obs, ref, sliding):
  26.         J, m, g = self.J, self.m, self.g
  27.         Ixx = J[0, 0]
  28.         Iyy = J[1, 1]
  29.         Izz = J[2, 2]
  30.         K = self.K
  31.         Kc = self.Kc
  32.         PHI = self.PHI
  33.         K1, K2, K3, K4 = K
  34.         k11, k12 = K1
  35.         k21, k22 = K2
  36.         k31, k32 = K3
  37.         k41, k42 = K4
  38.         kc1, kc2, kc3, kc4 = Kc
  39.         PHI1, PHI2, PHI3, PHI4 = PHI
  40.         # observation
  41.         obs = np.vstack((obs))
  42.         obs_ = np.vstack((obs[0:6],
  43.                           np.vstack(quat2angle(obs[6:10])[::-1]),
  44.                           obs[10:]))
  45.         x, y, z = obs_[0:3]
  46.         zd = obs_[5]
  47.         phi, theta, psi, phid, thetad, psid = obs_[6:]
  48.         # reference
  49.         ref = np.vstack((ref))
  50.         ref_ = np.vstack((ref[0:6],
  51.                           np.vstack(quat2angle(ref[6:10])[::-1]),
  52.                           ref[10:]))
  53.         x_r, y_r, z_r = ref_[0:3]
  54.         zd_r = ref_[5]
  55.         phi_r, theta_r, psi_r, phid_r, thetad_r, psid_r = ref_[6:]
  56.         # reference ddot term(z, phi, theta, psi)
  57.         F_r = m*g
  58.         M1_r = 0
  59.         M2_r = 0
  60.         M3_r = 0
  61.         zdd_r = g - cos(phi_r)*cos(theta_r)*F_r/m
  62.         phidd_r = (Iyy-Izz)/Ixx*thetad_r*psid_r + 1/Ixx*M1_r
  63.         thetadd_r = (Izz-Ixx)/Iyy*phid_r*psid_r + 1/Iyy*M2_r
  64.         psidd_r = (Ixx-Iyy)/Izz*phid_r*thetad_r + 1/Izz*M3_r
  65.         # error
  66.         e_x = x - x_r
  67.         e_y = y - y_r
  68.         e_z = z - z_r
  69.         e_zd = zd - zd_r
  70.         e_phi = phi - phi_r
  71.         e_phid = phid - phid_r
  72.         e_theta = theta - theta_r
  73.         e_thetad = thetad - thetad_r
  74.         e_psi = psi - psi_r
  75.         e_psid = psid - psid_r
  76.         # sliding surface
  77.         s1, s2, s3, s4 = sliding
  78.         # algebraic distance btw current state and boundary layer
  79.         dels1 = s1 - PHI1*sat(s1, PHI1)
  80.         dels2 = s2 - PHI2*sat(s2, PHI2)
  81.         dels3 = s3 - PHI3*sat(s3, PHI3)
  82.         dels4 = s4 - PHI4*sat(s4, PHI4)
  83.         # derivs
  84.         dP = np.vstack((e_x, e_y, e_z, e_phi, e_theta, e_psi))
  85.  
  86.         dgamma1 = -(- zdd_r + k12*e_zd + k11*e_z + g + kc1*sat(s1, PHI1))*dels1
  87.         # dgamma1 should have negative sign to make Lyapunov like func > 0
  88.         dgamma2 = (- phidd_r + k22*e_phid + k21*e_phi
  89.                    + (Iyy-Izz)/Ixx*thetad_r*psid_r + kc2*sat(s2, PHI2))*dels2
  90.         dgamma3 = (- thetadd_r + k32*e_thetad + k31*e_theta
  91.                    + (Izz-Ixx)/Iyy*phid_r*psid_r + kc3*sat(s3, PHI3))*dels3
  92.         dgamma4 = (- psidd_r + k42*e_psid + k41*e_psi
  93.                    + (Ixx-Iyy)/Izz*phid_r*thetad_r + kc4*sat(s4, PHI4))*dels4
  94.         dgamma = np.vstack((dgamma1, dgamma2, dgamma3, dgamma4))
  95.  
  96.         return dP, dgamma
  97.  
  98.     def set_dot(self, obs, ref, sliding):
  99.         dots = self.deriv(obs, ref, sliding)
  100.         self.P.dot, self.gamma.dot = dots
  101.         if self.gamma.state[0] > 0:
  102.             self.gamma.state[0] = 0
  103.         for i in range(3):
  104.             if self.gamma.state[i+1] < 0:
  105.                 self.gamma.state[i+1] = 0
  106.  
  107.     def get_FM(self, obs, ref, p, gamma, t):
  108.         K = np.array([[20, 15],
  109.                       [40, 20],
  110.                       [40, 20],
  111.                       [2, 1]])
  112.         Kc = np.vstack((10, 10, 15, 5))
  113.         PHI = np.vstack((0.8, 0.3, 0.3, 0.5))*0.1
  114.         self.K = K
  115.         self.Kc = Kc
  116.         self.PHI = PHI
  117.         p = np.vstack((p))
  118.         px, py, pz, pphi, ptheta, ppsi = p
  119.         gamma = np.vstack((gamma))
  120.         gamma1, gamma2, gamma3, gamma4 = gamma
  121.         K1, K2, K3, K4 = K
  122.         k11, k12 = K1
  123.         k21, k22 = K2
  124.         k31, k32 = K3
  125.         k41, k42 = K4
  126.         kc1, kc2, kc3, kc4 = Kc
  127.         PHI1, PHI2, PHI3, PHI4 = PHI
  128.         # model
  129.         J, g, m = self.J, self.g, self.m
  130.         Ixx = J[0, 0]
  131.         Iyy = J[1, 1]
  132.         Izz = J[2, 2]
  133.         # observation
  134.         obs = np.vstack((obs))
  135.         obs_ = np.vstack((obs[0:6],
  136.                           np.vstack(quat2angle(obs[6:10])[::-1]),
  137.                           obs[10:]))
  138.         x, y, z, xd, yd, zd = obs_[0:6]
  139.         phi, theta, psi, phid, thetad, psid = obs_[6:]
  140.         # reference
  141.         ref_ = np.vstack((ref[0:6],
  142.                           np.vstack(quat2angle(ref[6:10])[::-1]),
  143.                           ref[10:]))
  144.         x_r, y_r, z_r, xd_r, yd_r, zd_r = ref_[0:6]
  145.         phi_r, theta_r, psi_r, phid_r, thetad_r, psid_r = ref_[6:]
  146.         # reference ddot term(z, phi, theta, psi)
  147.         F_r = m*g
  148.         M1_r = 0
  149.         M2_r = 0
  150.         M3_r = 0
  151.         zdd_r = g - cos(phi_r)*cos(theta_r)*F_r/m
  152.         phidd_r = (Iyy-Izz)/Ixx*thetad_r*psid_r + 1/Ixx*M1_r
  153.         thetadd_r = (Izz-Ixx)/Iyy*phid_r*psid_r + 1/Iyy*M2_r
  154.         psidd_r = (Ixx-Iyy)/Izz*phid_r*thetad_r + 1/Izz*M3_r
  155.         # initial condition
  156.         z0, z0d = self.ic_[2], self.ic_[5]
  157.         phi0, theta0, psi0, phi0d, theta0d, psi0d = self.ic_[6:]
  158.         z0_r, z0d_r = self.ref0_[2], self.ref0_[5]
  159.         phi0_r, theta0_r, psi0_r, phi0d_r, theta0d_r, psi0d_r = self.ref0_[6:]
  160.         # PD control for position tracking (get phi_ref, theta_ref)
  161.         e_x = x - x_r
  162.         e_xd = xd - xd_r
  163.         e_y = y - y_r
  164.         e_yd = yd - yd_r
  165.         kp1, kd1, ki1 = np.array([0.25, 0.11, 0.045])*0.05
  166.         kp2, kd2, ki2 = np.array([0.25, 0.13, 0.03])*0.05
  167.         phi_r = -(kp1*e_y + kd1*e_yd + ki1*py)
  168.         theta_r = kp2*e_x + kd2*e_xd + ki2*px
  169.         # error definition
  170.         e_z = z - z_r
  171.         e_zd = zd - zd_r
  172.         e_phi = phi - phi_r
  173.         e_phid = phid - phid_r
  174.         e_theta = theta - theta_r
  175.         e_thetad = thetad - thetad_r
  176.         e_psi = psi - psi_r
  177.         e_psid = psid - psid_r
  178.         # sliding surface
  179.         s1 = e_zd + k12*e_z + k11*pz - k12*(z0-z0_r) - (z0d-z0d_r)
  180.         s2 = e_phid + k22*e_phi + k21*pphi - k22*(phi0-phi0_r) - (phi0d-phi0d_r)
  181.         s3 = e_thetad + k32*e_theta + k31*ptheta - k32*(theta0-theta0_r) \
  182.             - (theta0d-theta0d_r)
  183.         s4 = e_psid + k42*e_psi + k41*ppsi - k42*(psi0-psi0_r) - (psi0d-psi0d_r)
  184.         # get FM
  185.         F = gamma1*(zdd_r - k12*e_zd - k11*e_z - g) - gamma1*kc1*sat(s1, PHI1)
  186.         M1 = gamma2*(phidd_r - k22*e_phid - k21*e_phi - (Iyy-Izz)/Ixx*thetad*psid) \
  187.             - gamma2*kc2*sat(s2, PHI2)
  188.         M2 = gamma3*(thetadd_r - k32*e_thetad - k31*e_theta - (Izz-Ixx)/Iyy*phid*psid) \
  189.             - gamma3*kc3*sat(s3, PHI3)
  190.         M3 = gamma4*(psidd_r - k42*e_psid - k41*e_psi - (Ixx-Iyy)/Izz*phid*thetad) \
  191.             - gamma4*kc4*sat(s4, PHI4)
  192.  
  193.         action = np.vstack((F, M1, M2, M3))
  194.         sliding = np.vstack((s1, s2, s3, s4))
  195.  
  196.         return action, sliding

总体而言，类AdaptiveISMC_nonlinear同样实现了自适应滑模控制算法，用于解决多旋翼无人机在执行器故障情况下的控制问题。相较于类AdaptiveISMC_linear，类AdaptiveISMC_nonlinear包含了更加复杂的动力学模型和更多的修正措施，以适应非线性的系统特性。

未完待续

声明：本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：【wpsshop博客】
推荐阅读
相关标签

Copyright © 2003-2013 www.wpsshop.cn 版权所有，并保留所有权利。

  

闽ICP备14008679号