超级会员免费看
2.3 无人机运动控制系统
本项目是一个基于Python的无人机运动控制算法库，提供了多个算法模块，包括基于鲁棒控制的Adaptive ISMC、经典的ISMC、线性系统的LQR等。该库通过模拟无人机的运动和姿态控制，在不同环境和故障条件下评估算法性能。同时，项目还包括了详细的文档和测试文件，方便用户理解和验证算法实现。本项目包含如下所示的功能模块：
- Copter Models：提供了无人机的非线性和线性动力学模型，用于模拟不同控制算法的性能。
- Control Algorithms：包括鲁棒控制算法Adaptive ISMC、经典的ISMC、线性系统的LQR等，用于实现无人机的运动和姿态控制。
- Fault Injection：具备故障注入功能，允许在模拟中引入不同类型的故障，评估算法对故障的鲁棒性。
- Simulation Environment：提供了模拟环境，支持在不同条件下评估算法性能，包括故障处理、控制分配等。
- 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 fym2.3.2 控制算法
在本项目的“agents”目录中包含了4个Python程序文件，实现了对多旋翼无人机的不同控制算法，这些控制器旨在应对多旋翼无人机在不同工况下的位置和姿态控制问题，提高系统的鲁棒性和性能。
1. 被动失效（LoE）故障模型
文件decmk/agents/utils.py定义了一个名为LoE的类，代表了一种被动失效（LoE）故障模型。该类包含两个方法：get_effectiveness用于获取失效模型的效果，get用于应用失效模型到输入信号上。在测试代码中，创建了一个LoE对象，设置了失效时间、失效位置和失效水平，然后演示了获取失效效果和应用失效模型的过程。
import numpy as np
class LoE:
def __init__(self, time=0, index=0, level=1.0):
self.time = time
self.index = index
self.level = level
def get(self, t, u):
effectiveness = self.get_effectiveness(t, np.ones(u.shape[0]))
return u * effectiveness.reshape(u.shape[0], 1)
def get_effectiveness(self, t, effectiveness):
if t >= self.time:
effectiveness[self.index] = self.level
return effectiveness
if __name__ == "__main__":
t = 4
loe = LoE(time=3, index=0, level=0.5)
print(loe.get_effectiveness(t, np.array([1, 2, 3, 4])))
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的结果。这样的函数通常用于限制变量在特定范围内波动。
def sat(s, eps):
if s > eps:
return 1
elif s < -eps:
return -1
else:
return s/eps（2）类IntegralSMC_linear 是基于自适应容错滑模控制方案的多旋翼无人机控制器，该类包含以下函数：
- __init__(self, J, m, g, d, ic, ref0)：初始化函数，设置控制器的初始参数和状态。
- deriv(self, obs, ref)：计算状态误差的导数，用于控制器设计。
- set_dot(self, obs, ref)：设置状态误差的导数。
- get_FM(self, obs, ref, p, K, Kc, PHI, t)：根据当前状态和参考状态计算控制力和滑模面，实现自适应容错滑模控制，返回控制力和滑模面。其中，参数obs表示当前状态，ref表示参考状态，p表示控制器参数，K表示PD控制器增益，Kc表示滑模面控制增益，PHI表示滑模面参数，t表示时间。
class IntegralSMC_linear(BaseEnv):
'''
参考文献
Ban Wang, Youmin Zhang“一种自适应容错滑模控制分配方案用于多旋翼无人机受执行器故障影响”，IEEE工业电子学报，第65卷，第5期，2018年5月
'''
def __init__(self, J, m, g, d, ic, ref0):
super().__init__()
self.ic = ic
self.ref0 = ref0
self.J, self.m, self.g, self.d = J, m, g, d
# x、y、z、roll、pitch、yaw的误差积分
self.P = BaseSystem(np.vstack((self.ic[0] - self.ref0[0],
self.ic[1] - self.ref0[0],
self.ic[2] - self.ref0[2],
self.ic[6] - self.ref0[6],
self.ic[7] - self.ref0[7],
self.ic[8] - self.ref0[8])))
def deriv(self, obs, ref):
# 观测
obs = np.vstack((obs))
x, y, z = obs[0:3]
phi, theta, psi = obs[6:9]
# 参考
ref = np.vstack((ref))
x_r, y_r, z_r = ref[0:3]
phi_r, theta_r, psi_r = ref[6:9]
dP = np.vstack((x - x_r,
y - y_r,
z - z_r,
phi - phi_r,
theta - theta_r,
psi - psi_r))
return dP
def set_dot(self, obs, ref):
dot = self.deriv(obs, ref)
self.P.dot = dot
def get_FM(self, obs, ref, p, K, Kc, PHI, t):
p = np.vstack((p))
px, py, pz, pphi, ptheta, ppsi = p
K1, K2, K3, K4 = K
k11, k12 = K1
k21, k22 = K2
k31, k32 = K3
k41, k42 = K4
kc1, kc2, kc3, kc4 = Kc
PHI1, PHI2, PHI3, PHI4 = PHI
# 模型
J = self.J
Ixx = J[0, 0]
Iyy = J[1, 1]
Izz = J[2, 2]
m, g, d = self.m, self.g, self.d
# 观测
obs = np.vstack((obs))
x, y, z, xd, yd, zd = obs[0:6]
phi, theta, psi, phid, thetad, psid = obs[6:]
# 参考
ref = np.vstack((ref))
x_r, y_r, z_r, xd_r, yd_r, zd_r = ref[0:6]
phi_r, theta_r, psi_r, phid_r, thetad_r, psid_r = ref[6:]
zdd_r = 0
phidd_r = 0
thetadd_r = 0
psidd_r = 0
# 初始条件
z0, z0d = self.ic[2], self.ic[5]
phi0, theta0, psi0, phi0d, theta0d, psi0d = self.ic[6:]
z0_r, z0d_r = self.ref0[2], self.ref0[5]
phi0_r, theta0_r, psi0_r, phi0d_r, theta0d_r, psi0d_r = self.ref0[6:]
# 位置跟踪的PD控制（获取phi_ref、theta_ref）
e_x = x - x_r
e_xd = xd - xd_r
e_y = y - y_r
e_yd = yd - yd_r
kp1, kd1, ki1 = np.array([0.5, 0.3, 0.2])
kp2, kd2, ki2 = np.array([0.5, 0.3, 0.2])
phi_r = -(kp1*e_y + kd1*e_yd + ki1*py)
theta_r = kp2*e_x + kd2*e_xd + ki2*px
# 误差定义
e_z = z - z_r
e_zd = zd - zd_r
e_phi = phi - phi_r
e_phid = phid - phid_r
e_theta = theta - theta_r
e_thetad = thetad - thetad_r
e_psi = psi - psi_r
e_psid = psid - psid_r
# h**(-1)函数定义
h1 = -m/cos(phi)/cos(theta)
h2 = Ixx/d
h3 = Iyy/d
h4 = Izz
# 滑模面
s1 = e_zd + k12*e_z + k11*pz - k12*(z0-z0_r) - (z0d-z0d_r)
s2 = e_phid + k22*e_phi + k21*pphi - k22*(phi0-phi0_r) - (phi0d-phi0d_r)
s3 = e_thetad + k32*e_theta + k31*ptheta - k32*(theta0-theta0_r) - (theta0d-theta0d_r)
s4 = e_psid + k42*e_psi + k41*ppsi - k42*(psi0-psi0_r) - (psi0d-psi0d_r)
# 获取FM
F = h1*(zdd_r - k12*e_zd - k11*e_z - g) - h1*kc1*sat(s1, PHI1)
M1 = h2*(phidd_r - k22*e_phid - k21*e_phi - (Iyy-Izz)/Ixx*thetad*psid) \
- h2*kc2*sat(s2, PHI2)
M2 = h3*(thetadd_r - k32*e_thetad - k31*e_theta
- (Izz-Ixx)/Iyy*phid*psid) - h3*kc3*sat(s3, PHI3)
M3 = h4*(psidd_r - k42*e_psid - k41*e_psi - (Ixx-Iyy)/Izz*phid*thetad) \
- h4*kc4*sat(s4, PHI4)
action = np.vstack((F, M1, M2, M3))
sliding = np.vstack((s1, s2, s3, s4))
return action, sliding（3）类IntegralSMC_nonlinear 是一个基于自适应容错滑模控制（Adaptive Fault-Tolerant Sliding Mode Control）方案的多旋翼无人机控制环境，该类实现了一个多旋翼无人机的自适应容错滑模控制器，通过对状态变量的控制力和力矩进行计算，实现对无人机运动的控制。类IntegralSMC_nonlinear 中的主要函数及其功能如下所示。
- __init__(self, J, m, g, d, ic, ref0)：初始化函数，设置控制环境的初始条件和参考值。
- deriv(self, obs, ref)：计算状态变量的导数，用于控制器的微分部分。
- set_dot(self, obs, ref)：设置状态变量的导数，用于更新控制器的微分项。
- get_FM(self, obs, ref, p, t)：根据当前状态、参考状态和控制参数，计算多旋翼无人机的控制力和力矩。其中，obs 是当前状态，ref 是参考状态，p 是控制参数，t 是时间。
class IntegralSMC_nonlinear(BaseEnv):
'''
参考文献
Ban Wang, Youmin Zhang，“一种自适应容错滑模控制分配方案用于多旋翼无人机受执行器故障影响”，IEEE工业电子学报，第65卷，第5期，2018年5月
'''
def __init__(self, J, m, g, d, ic, ref0):
super().__init__()
self.ic_ = np.vstack((ic[0:6], np.vstack(quat2angle(ic[6:10])[::-1]), ic[10:]))
self.ref0_ = np.vstack((ref0[0:6], np.vstack(quat2angle(ref0[6:10])[::-1]), ref0[10:]))
self.J, self.m, self.g, self.d = J, m, g, d
# x、y、z、roll、pitch、yaw的误差积分
self.P = BaseSystem(np.vstack((self.ic_[0] - self.ref0_[0],
self.ic_[1] - self.ref0_[1],
self.ic_[2] - self.ref0_[2],
self.ic_[6] - self.ref0_[6],
self.ic_[7] - self.ref0_[7],
self.ic_[8] - self.ref0_[8])))
def deriv(self, obs, ref):
# 观测
obs = np.vstack((obs))
obs_ = np.vstack((obs[0:6], np.vstack(quat2angle(obs[6:10])[::-1]), obs[10:]))
x, y, z = obs_[0:3]
phi, theta, psi = obs_[6:9]
# 参考
ref = np.vstack((ref))
ref_ = np.vstack((ref[0:6], np.vstack(quat2angle(ref[6:10])[::-1]), ref[10:]))
x_r, y_r, z_r = ref_[0:3]
phi_r, theta_r, psi_r = ref_[6:9]
dP = np.vstack((x - x_r,
y - y_r,
z - z_r,
phi - phi_r,
theta - theta_r,
psi - psi_r))
return dP
def set_dot(self, obs, ref):
dot = self.deriv(obs, ref)
self.P.dot = dot
def get_FM(self, obs, ref, p, t):
K = np.array([[25, 15],
[40, 20],
[40, 20],
[1, 1]])
Kc = np.vstack((15, 15, 15, 10))
PHI = np.vstack((0.8, 0.1, 0.1, 1))
p = np.vstack((p))
px, py, pz, pphi, ptheta, ppsi = p
K1, K2, K3, K4 = K
k11, k12 = K1
k21, k22 = K2
k31, k32 = K3
k41, k42 = K4
kc1, kc2, kc3, kc4 = Kc
PHI1, PHI2, PHI3, PHI4 = PHI
# 模型
J = self.J
Ixx = J[0, 0]
Iyy = J[1, 1]
Izz = J[2, 2]
m, g, d = self.m, self.g, self.d
# 观测
obs = np.vstack((obs))
obs_ = np.vstack((obs[0:6], np.vstack(quat2angle(obs[6:10])[::-1]), obs[10:]))
x, y, z, xd, yd, zd = obs_[0:6]
phi, theta, psi, phid, thetad, psid = obs_[6:]
# 参考
ref_ = np.vstack((ref[0:6], np.vstack(quat2angle(ref[6:10])[::-1]), ref[10:]))
x_r, y_r, z_r, xd_r, yd_r, zd_r = ref_[0:6]
phi_r, theta_r, psi_r, phid_r, thetad_r, psid_r = ref_[6:]
zdd_r = 0
phidd_r = 0
thetadd_r = 0
psidd_r = 0
# 初始条件
z0, z0d = self.ic_[2], self.ic_[5]
phi0, theta0, psi0, phi0d, theta0d, psi0d = self.ic_[6:]
z0_r, z0d_r = self.ref0_[2], self.ref0_[5]
phi0_r, theta0_r, psi0_r, phi0d_r, theta0d_r, psi0d_r = self.ref0_[6:]
# 位置跟踪的PD控制（获取phi_ref、theta_ref）
e_x = x - x_r
e_xd = xd - xd_r
e_y = y - y_r
e_yd = yd - yd_r
kp1, kd1, ki1 = np.array([0.25, 0.11, 0.045])
kp2, kd2, ki2 = np.array([0.25, 0.13, 0.03])
phi_r = -(kp1*e_y + kd1*e_yd + ki1*py)
theta_r = kp2*e_x + kd2*e_xd + ki2*px
# 误差定义
e_z = z - z_r
e_zd = zd - zd_r
e_phi = phi - phi_r
e_phid = phid - phid_r
e_theta = theta - theta_r
e_thetad = thetad - thetad_r
e_psi = psi - psi_r
e_psid = psid - psid_r
# h**(-1)函数定义
h1 = -m/cos(phi)/cos(theta)
h2 = Ixx/d
h3 = Iyy/d
h4 = Izz
# 滑模面
s1 = e_zd + k12*e_z + k11*pz - k12*(z0-z0_r) - (z0d-z0d_r)
s2 = e_phid + k22*e_phi + k21*pphi - k22*(phi0-phi0_r) - (phi0d-phi0d_r)
s3 = e_thetad + k32*e_theta + k31*ptheta - k32*(theta0-theta0_r) - (theta0d-theta0d_r)
s4 = e_psid + k42*e_psi + k41*ppsi - k42*(psi0-psi0_r) - (psi0d-psi0d_r)
# 获取FM
F = h1*(zdd_r - k12*e_zd - k11*e_z - g) - h1*kc1*sat(s1, PHI1)
M1 = h2*(phidd_r - k22*e_phid - k21*e_phi - (Iyy-Izz)/Ixx*thetad*psid) - h2*kc2*sat(s2, PHI2)
M2 = h3*(thetadd_r - k32*e_thetad - k31*e_theta - (Izz-Ixx)/Iyy*phid*psid) - h3*kc3*sat(s3, PHI3)
M3 = h4*(psidd_r - k42*e_psid - k41*e_psi - (Ixx-Iyy)/Izz*phid*thetad) - h4*kc4*sat(s4, PHI4)
action = np.vstack((F, M1, M2, M3))
return action3. 最优控制算法
文件decmk/agents/lqr.py定义了一个线性二次型调节器（LQR）控制器和相应的控制类LQRController。LQR控制器通过计算系统状态和参考状态之间的差异，并应用调整力矩，实现对多旋翼无人机的姿态和位置控制。控制器中包括状态转换函数、初始化函数以及计算控制力和力矩的函数。LQR（线性二次型调节器）是最优控制中的一种经典算法。它基于状态空间模型，通过最小化状态与控制输入的二次代价函数来设计控制器，从而实现系统的最优控制。LQR在工程控制领域广泛应用，尤其在线性系统和近似线性系统的控制中表现出色。
def LQR(A: np.array, B: np.array, Q: np.array, R: np.array, with_eigs=False) \
-> np.array:
P = lin.solve_continuous_are(A, B, Q, R)
if np.size(R) == 1:
K = (np.transpose(B).dot(P)) / R
else:
K = np.linalg.inv(R).dot((np.transpose(B).dot(P)))
eig_vals, eig_vecs = np.linalg.eig(A - B.dot(K))
if with_eigs:
return K, P, eig_vals, eig_vecs
else:
return K, P
class LQRController:
def __init__(self, Jinv, m, g,
Q=np.diag([1, 1, 1, 0, 0, 0, 1, 1, 1, 0, 0, 0]),
R=np.diag([1, 1, 1, 1]),
):
self.Jinv = Jinv
self.m, self.g = m, g
self.trim_forces = np.vstack([self.m * self.g, 0, 0, 0])
'''
linearized condition near trim pt
state variables with {pos, vel, Euler angle, omega}
'''
A = np.array([[0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, -g, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, g, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]])
B = np.array([[0, 0, 0, 0],
[0, 0, 0, 0],
[0, 0, 0, 0],
[0, 0, 0, 0],
[0, 0, 0, 0],
[-1/m, 0, 0, 0],
[0, 0, 0, 0],
[0, 0, 0, 0],
[0, 0, 0, 0],
[0, Jinv[0, 0], 0, 0],
[0, 0, Jinv[1, 1], 0],
[0, 0, 0, Jinv[2, 2]]])
self.K, *_ = LQR(A, B, Q, R)
def transform(self, y):
# input comes in a form {pos, vel, quat, omega}
if len(y) == 13:
return np.vstack((y[0:6],
np.vstack(quat2angle(y[6:10])[::-1]), y[10:]))
def get_FM(self, obs, ref):
x = self.transform(obs)
x_ref = self.transform(ref)
forces = -self.K.dot(x - x_ref) + self.trim_forces
return forces4. 自适应滑模控制（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 是一个多旋翼无人机控制器，基于自适应滑模控制方法，旨在处理多旋翼无人机在存在执行器故障的情况下的位置和姿态控制。该类包含了如下所示的功能函数：
- __init__(self, J, m, g, d, ic, ref0)：类的初始化函数，设置控制器的初始参数和系统状态。
- deriv(self, obs, ref, sliding)：计算多旋翼无人机系统的状态变量导数，用于模拟系统的动态行为。函数接受当前状态、参考状态和滑动表面作为输入，返回状态导数。
- set_dot(self, obs, ref, sliding)：根据 deriv 函数计算的状态导数，更新控制器内部的状态变量。
- get_FM(self, obs, ref, p, gamma, K, Kc, PHI, t)：根据当前状态、参考状态以及控制器的参数和状态，计算多旋翼无人机的控制输入和滑动表面。返回计算得到的控制输入和滑动表面。
class AdaptiveISMC_linear(BaseEnv):
def __init__(self, J, m, g, d, ic, ref0):
super().__init__()
self.ic = ic
self.ref0 = ref0
self.J, self.m, self.g, self.d = J, m, g, d
# error integral of x, y, z, roll, pitch, yaw
self.P = BaseSystem(np.vstack((self.ic[0] - self.ref0[0],
self.ic[1] - self.ref0[1],
self.ic[2] - self.ref0[2],
self.ic[6] - self.ref0[6],
self.ic[7] - self.ref0[7],
self.ic[8] - self.ref0[8])))
# parameter update, estimation
self.gamma = BaseSystem(np.vstack((-m/cos(ic[6])/cos(ic[7]),
J[0, 0]/d,
J[1, 1]/d,
J[2, 2])))
def deriv(self, obs, ref, sliding):
J, m, g = self.J, self.m, self.g
Ixx = J[0, 0]
Iyy = J[1, 1]
Izz = J[2, 2]
K = self.K
Kc = self.Kc
PHI = self.PHI
K1, K2, K3, K4 = K
k11, k12 = K1
k21, k22 = K2
k31, k32 = K3
k41, k42 = K4
kc1, kc2, kc3, kc4 = Kc
PHI1, PHI2, PHI3, PHI4 = PHI
# observation
obs = np.vstack((obs))
x, y, z = obs[0:3]
zd = obs[5]
phi, theta, psi, phid, thetad, psid = obs[6:]
# reference
ref = np.vstack((ref))
x_r, y_r, z_r = ref[0:3]
zd_r = ref[5]
phi_r, theta_r, psi_r, phid_r, thetad_r, psid_r = ref[6:]
# reference ddot term(z, phi, theta, psi)
F_r = m*g
M1_r = 0
M2_r = 0
M3_r = 0
zdd_r = g - cos(phi_r)*cos(theta_r)*F_r/m
phidd_r = (Iyy-Izz)/Ixx*thetad_r*psid_r + 1/Ixx*M1_r
thetadd_r = (Izz-Ixx)/Iyy*phid_r*psid_r + 1/Iyy*M2_r
psidd_r = (Ixx-Iyy)/Izz*phid_r*thetad_r + 1/Izz*M3_r
# error
e_x = x - x_r
e_y = y - y_r
e_z = z - z_r
e_zd = zd - zd_r
e_phi = phi - phi_r
e_phid = phid - phid_r
e_theta = theta - theta_r
e_thetad = thetad - thetad_r
e_psi = psi - psi_r
e_psid = psid - psid_r
# sliding surface
s1, s2, s3, s4 = sliding
# algebraic distance btw current state and boundary layer
dels1 = s1 - PHI1*sat(s1, PHI1)
dels2 = s2 - PHI2*sat(s2, PHI2)
dels3 = s3 - PHI3*sat(s3, PHI3)
dels4 = s4 - PHI4*sat(s4, PHI4)
# derivs
dP = np.vstack((e_x, e_y, e_z, e_phi, e_theta, e_psi))
dgamma1 = -(- zdd_r + k12*e_zd + k11*e_z + g + kc1*sat(s1, PHI1))*dels1
# dgamma1 should have negative sign to make Lyapunov like func > 0
dgamma2 = (- phidd_r + k22*e_phid + k21*e_phi
+ (Iyy-Izz)/Ixx*thetad_r*psid_r + kc2*sat(s2, PHI2))*dels2
dgamma3 = (- thetadd_r + k32*e_thetad + k31*e_theta
+ (Izz-Ixx)/Iyy*phid_r*psid_r + kc3*sat(s3, PHI3))*dels3
dgamma4 = (- psidd_r + k42*e_psid + k41*e_psi
+ (Ixx-Iyy)/Izz*phid_r*thetad_r + kc4*sat(s4, PHI4))*dels4
dgamma = np.vstack((dgamma1, dgamma2, dgamma3, dgamma4))
return dP, dgamma
def set_dot(self, obs, ref, sliding):
dots = self.deriv(obs, ref, sliding)
self.P.dot, self.gamma.dot = dots
def get_FM(self, obs, ref, p, gamma, K, Kc, PHI, t):
self.K = K
self.Kc = Kc
self.PHI = PHI
p = np.vstack((p))
px, py, pz, pphi, ptheta, ppsi = p
gamma = np.vstack((gamma))
gamma1, gamma2, gamma3, gamma4 = gamma
K1, K2, K3, K4 = K
k11, k12 = K1
k21, k22 = K2
k31, k32 = K3
k41, k42 = K4
kc1, kc2, kc3, kc4 = Kc
PHI1, PHI2, PHI3, PHI4 = PHI
# model
J, g, m = self.J, self.g, self.m
Ixx = J[0, 0]
Iyy = J[1, 1]
Izz = J[2, 2]
# observation
obs = np.vstack((obs))
x, y, z, xd, yd, zd = obs[0:6]
phi, theta, psi, phid, thetad, psid = obs[6:]
# reference
x_r, y_r, z_r, xd_r, yd_r, zd_r = ref[0:6]
phi_r, theta_r, psi_r, phid_r, thetad_r, psid_r = ref[6:]
# reference ddot term(z, phi, theta, psi)
F_r = m*g
M1_r = 0
M2_r = 0
M3_r = 0
zdd_r = g - cos(phi_r)*cos(theta_r)*F_r/m
phidd_r = (Iyy-Izz)/Ixx*thetad_r*psid_r + 1/Ixx*M1_r
thetadd_r = (Izz-Ixx)/Iyy*phid_r*psid_r + 1/Iyy*M2_r
psidd_r = (Ixx-Iyy)/Izz*phid_r*thetad_r + 1/Izz*M3_r
# initial condition
z0, z0d = self.ic[2], self.ic[5]
phi0, theta0, psi0, phi0d, theta0d, psi0d = self.ic[6:]
z0_r, z0d_r = self.ref0[2], self.ref0[5]
phi0_r, theta0_r, psi0_r, phi0d_r, theta0d_r, psi0d_r = self.ref0[6:]
# PD control for position tracking (get phi_ref, theta_ref)
e_x = x - x_r
e_xd = xd - xd_r
e_y = y - y_r
e_yd = yd - yd_r
kp1, kd1, ki1 = np.array([0.3, 0.2, 0.1])
kp2, kd2, ki2 = np.array([0.3, 0.2, 0.1])
phi_r = -(kp1*e_y + kd1*e_yd + ki1*py)
theta_r = kp2*e_x + kd2*e_xd + ki2*px
# error definition
e_z = z - z_r
e_zd = zd - zd_r
e_phi = phi - phi_r
e_phid = phid - phid_r
e_theta = theta - theta_r
e_thetad = thetad - thetad_r
e_psi = psi - psi_r
e_psid = psid - psid_r
# sliding surface
s1 = e_zd + k12*e_z + k11*pz - k12*(z0-z0_r) - (z0d-z0d_r)
s2 = e_phid + k22*e_phi + k21*pphi - k22*(phi0-phi0_r) - (phi0d-phi0d_r)
s3 = e_thetad + k32*e_theta + k31*ptheta - k32*(theta0-theta0_r) \
- (theta0d-theta0d_r)
s4 = e_psid + k42*e_psi + k41*ppsi - k42*(psi0-psi0_r) - (psi0d-psi0d_r)
# get FM
F = gamma1*(zdd_r - k12*e_zd - k11*e_z - g) - gamma1*kc1*sat(s1, PHI1)
M1 = gamma2*(phidd_r - k22*e_phid - k21*e_phi - (Iyy-Izz)/Ixx*thetad*psid) \
- gamma2*kc2*sat(s2, PHI2)
M2 = gamma3*(thetadd_r - k32*e_thetad - k31*e_theta - (Izz-Ixx)/Iyy*phid*psid) \
- gamma3*kc3*sat(s3, PHI3)
M3 = gamma4*(psidd_r - k42*e_psid - k41*e_psi - (Ixx-Iyy)/Izz*phid*thetad) \
- gamma4*kc4*sat(s4, PHI4)
action = np.vstack((F, M1, M2, M3))
sliding = np.vstack((s1, s2, s3, s4))
return action, sliding（2）类 AdaptiveISMC_nonlinear 也是一个多旋翼无人机控制器，同样基于自适应滑模控制方法，专门设计用于处理多旋翼无人机在存在执行器故障的情况下的位置和姿态控制。在类 AdaptiveISMC_nonlinear包含如下所示的内置函数。
- __init__(self, J, m, g, d, ic, ref0)：类的初始化函数，设置控制器的初始参数和系统状态。
- deriv(self, obs, ref, sliding)：计算多旋翼无人机系统的状态变量导数，用于模拟系统的动态行为。函数接受当前状态、参考状态和滑动表面作为输入，返回状态导数。
- set_dot(self, obs, ref, sliding)：根据 deriv 函数计算的状态导数，更新控制器内部的状态变量。特别地，在这个类中，还包括对控制器状态的一些修正。
- get_FM(self, obs, ref, p, gamma, t)：根据当前状态、参考状态以及控制器的参数和状态，计算多旋翼无人机的控制输入和滑动表面。返回计算得到的控制输入和滑动表面。
class AdaptiveISMC_nonlinear(BaseEnv):
def __init__(self, J, m, g, d, ic, ref0):
super().__init__()
self.ic_ = np.vstack((ic[0:6],
np.vstack(quat2angle(ic[6:10])[::-1]),
ic[10:]))
self.ref0_ = np.vstack((ref0[0:6],
np.vstack(quat2angle(ref0[6:10])[::-1]),
ref0[10:]))
self.J, self.m, self.g, self.d = J, m, g, d
# error integral of x, y, z, roll, pitch, yaw
self.P = BaseSystem(np.vstack((self.ic_[0] - self.ref0_[0],
self.ic_[1] - self.ref0_[1],
self.ic_[2] - self.ref0_[2],
self.ic_[6] - self.ref0_[6],
self.ic_[7] - self.ref0_[7],
self.ic_[8] - self.ref0_[8])))
# parameter update, estimation
self.gamma = BaseSystem(np.vstack((-m/cos(self.ic_[6])/cos(self.ic_[7]),
J[0, 0]/d,
J[1, 1]/d,
J[2, 2])))
def deriv(self, obs, ref, sliding):
J, m, g = self.J, self.m, self.g
Ixx = J[0, 0]
Iyy = J[1, 1]
Izz = J[2, 2]
K = self.K
Kc = self.Kc
PHI = self.PHI
K1, K2, K3, K4 = K
k11, k12 = K1
k21, k22 = K2
k31, k32 = K3
k41, k42 = K4
kc1, kc2, kc3, kc4 = Kc
PHI1, PHI2, PHI3, PHI4 = PHI
# observation
obs = np.vstack((obs))
obs_ = np.vstack((obs[0:6],
np.vstack(quat2angle(obs[6:10])[::-1]),
obs[10:]))
x, y, z = obs_[0:3]
zd = obs_[5]
phi, theta, psi, phid, thetad, psid = obs_[6:]
# reference
ref = np.vstack((ref))
ref_ = np.vstack((ref[0:6],
np.vstack(quat2angle(ref[6:10])[::-1]),
ref[10:]))
x_r, y_r, z_r = ref_[0:3]
zd_r = ref_[5]
phi_r, theta_r, psi_r, phid_r, thetad_r, psid_r = ref_[6:]
# reference ddot term(z, phi, theta, psi)
F_r = m*g
M1_r = 0
M2_r = 0
M3_r = 0
zdd_r = g - cos(phi_r)*cos(theta_r)*F_r/m
phidd_r = (Iyy-Izz)/Ixx*thetad_r*psid_r + 1/Ixx*M1_r
thetadd_r = (Izz-Ixx)/Iyy*phid_r*psid_r + 1/Iyy*M2_r
psidd_r = (Ixx-Iyy)/Izz*phid_r*thetad_r + 1/Izz*M3_r
# error
e_x = x - x_r
e_y = y - y_r
e_z = z - z_r
e_zd = zd - zd_r
e_phi = phi - phi_r
e_phid = phid - phid_r
e_theta = theta - theta_r
e_thetad = thetad - thetad_r
e_psi = psi - psi_r
e_psid = psid - psid_r
# sliding surface
s1, s2, s3, s4 = sliding
# algebraic distance btw current state and boundary layer
dels1 = s1 - PHI1*sat(s1, PHI1)
dels2 = s2 - PHI2*sat(s2, PHI2)
dels3 = s3 - PHI3*sat(s3, PHI3)
dels4 = s4 - PHI4*sat(s4, PHI4)
# derivs
dP = np.vstack((e_x, e_y, e_z, e_phi, e_theta, e_psi))
dgamma1 = -(- zdd_r + k12*e_zd + k11*e_z + g + kc1*sat(s1, PHI1))*dels1
# dgamma1 should have negative sign to make Lyapunov like func > 0
dgamma2 = (- phidd_r + k22*e_phid + k21*e_phi
+ (Iyy-Izz)/Ixx*thetad_r*psid_r + kc2*sat(s2, PHI2))*dels2
dgamma3 = (- thetadd_r + k32*e_thetad + k31*e_theta
+ (Izz-Ixx)/Iyy*phid_r*psid_r + kc3*sat(s3, PHI3))*dels3
dgamma4 = (- psidd_r + k42*e_psid + k41*e_psi
+ (Ixx-Iyy)/Izz*phid_r*thetad_r + kc4*sat(s4, PHI4))*dels4
dgamma = np.vstack((dgamma1, dgamma2, dgamma3, dgamma4))
return dP, dgamma
def set_dot(self, obs, ref, sliding):
dots = self.deriv(obs, ref, sliding)
self.P.dot, self.gamma.dot = dots
if self.gamma.state[0] > 0:
self.gamma.state[0] = 0
for i in range(3):
if self.gamma.state[i+1] < 0:
self.gamma.state[i+1] = 0
def get_FM(self, obs, ref, p, gamma, t):
K = np.array([[20, 15],
[40, 20],
[40, 20],
[2, 1]])
Kc = np.vstack((10, 10, 15, 5))
PHI = np.vstack((0.8, 0.3, 0.3, 0.5))*0.1
self.K = K
self.Kc = Kc
self.PHI = PHI
p = np.vstack((p))
px, py, pz, pphi, ptheta, ppsi = p
gamma = np.vstack((gamma))
gamma1, gamma2, gamma3, gamma4 = gamma
K1, K2, K3, K4 = K
k11, k12 = K1
k21, k22 = K2
k31, k32 = K3
k41, k42 = K4
kc1, kc2, kc3, kc4 = Kc
PHI1, PHI2, PHI3, PHI4 = PHI
# model
J, g, m = self.J, self.g, self.m
Ixx = J[0, 0]
Iyy = J[1, 1]
Izz = J[2, 2]
# observation
obs = np.vstack((obs))
obs_ = np.vstack((obs[0:6],
np.vstack(quat2angle(obs[6:10])[::-1]),
obs[10:]))
x, y, z, xd, yd, zd = obs_[0:6]
phi, theta, psi, phid, thetad, psid = obs_[6:]
# reference
ref_ = np.vstack((ref[0:6],
np.vstack(quat2angle(ref[6:10])[::-1]),
ref[10:]))
x_r, y_r, z_r, xd_r, yd_r, zd_r = ref_[0:6]
phi_r, theta_r, psi_r, phid_r, thetad_r, psid_r = ref_[6:]
# reference ddot term(z, phi, theta, psi)
F_r = m*g
M1_r = 0
M2_r = 0
M3_r = 0
zdd_r = g - cos(phi_r)*cos(theta_r)*F_r/m
phidd_r = (Iyy-Izz)/Ixx*thetad_r*psid_r + 1/Ixx*M1_r
thetadd_r = (Izz-Ixx)/Iyy*phid_r*psid_r + 1/Iyy*M2_r
psidd_r = (Ixx-Iyy)/Izz*phid_r*thetad_r + 1/Izz*M3_r
# initial condition
z0, z0d = self.ic_[2], self.ic_[5]
phi0, theta0, psi0, phi0d, theta0d, psi0d = self.ic_[6:]
z0_r, z0d_r = self.ref0_[2], self.ref0_[5]
phi0_r, theta0_r, psi0_r, phi0d_r, theta0d_r, psi0d_r = self.ref0_[6:]
# PD control for position tracking (get phi_ref, theta_ref)
e_x = x - x_r
e_xd = xd - xd_r
e_y = y - y_r
e_yd = yd - yd_r
kp1, kd1, ki1 = np.array([0.25, 0.11, 0.045])*0.05
kp2, kd2, ki2 = np.array([0.25, 0.13, 0.03])*0.05
phi_r = -(kp1*e_y + kd1*e_yd + ki1*py)
theta_r = kp2*e_x + kd2*e_xd + ki2*px
# error definition
e_z = z - z_r
e_zd = zd - zd_r
e_phi = phi - phi_r
e_phid = phid - phid_r
e_theta = theta - theta_r
e_thetad = thetad - thetad_r
e_psi = psi - psi_r
e_psid = psid - psid_r
# sliding surface
s1 = e_zd + k12*e_z + k11*pz - k12*(z0-z0_r) - (z0d-z0d_r)
s2 = e_phid + k22*e_phi + k21*pphi - k22*(phi0-phi0_r) - (phi0d-phi0d_r)
s3 = e_thetad + k32*e_theta + k31*ptheta - k32*(theta0-theta0_r) \
- (theta0d-theta0d_r)
s4 = e_psid + k42*e_psi + k41*ppsi - k42*(psi0-psi0_r) - (psi0d-psi0d_r)
# get FM
F = gamma1*(zdd_r - k12*e_zd - k11*e_z - g) - gamma1*kc1*sat(s1, PHI1)
M1 = gamma2*(phidd_r - k22*e_phid - k21*e_phi - (Iyy-Izz)/Ixx*thetad*psid) \
- gamma2*kc2*sat(s2, PHI2)
M2 = gamma3*(thetadd_r - k32*e_thetad - k31*e_theta - (Izz-Ixx)/Iyy*phid*psid) \
- gamma3*kc3*sat(s3, PHI3)
M3 = gamma4*(psidd_r - k42*e_psid - k41*e_psi - (Ixx-Iyy)/Izz*phid*thetad) \
- gamma4*kc4*sat(s4, PHI4)
action = np.vstack((F, M1, M2, M3))
sliding = np.vstack((s1, s2, s3, s4))
return action, sliding总体而言，类AdaptiveISMC_nonlinear同样实现了自适应滑模控制算法，用于解决多旋翼无人机在执行器故障情况下的控制问题。相较于类AdaptiveISMC_linear，类AdaptiveISMC_nonlinear包含了更加复杂的动力学模型和更多的修正措施，以适应非线性的系统特性。
扫一扫
934
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
