2.3.5 ?基于自適應(yīng)反演滑?？刂破鞯姆抡鏈y試
文件test/fault_AISMC.py實現(xiàn)了一個基于非線性動力學(xué)模型的無人機飛行控制仿真環(huán)境，通過使用自適應(yīng)反演滑?？刂破鳎ˋdaptive Inverse Sliding Mode Control，AdaptiveISMC_nonlinear）對無人機進行控制，并引入了執(zhí)行器故障模型以模擬實際飛行中可能發(fā)生的故障情況。最后可視化仿真結(jié)果，展示了無人機在控制算法下的軌跡、執(zhí)行器狀態(tài)、故障檢測與隔離等信息，用于分析控制器對飛行器動態(tài)響應(yīng)及故障情況的影響。
文件test/fault_AISMC.py的具體實現(xiàn)流程如下所示。
（1）函數(shù)Env.init()用于初始化仿真環(huán)境，包括定義執(zhí)行器故障（actuator_faults）、初始條件（ic）、參考軌跡（ref0）以及創(chuàng)建無人機模型（Copter_nonlinear）、執(zhí)行器故障檢測與隔離模塊（FDI）和自適應(yīng)反演滑?？刂破鳎ˋdaptiveISMC_nonlinear）。
class Env(BaseEnv):
def __init__(self):
super().__init__(solver="odeint", max_t=20, dt=10, ode_step_len=100)
# Define faults
self.actuator_faults = [
LoE(time=5, index=0, level=0.0),
# LoE(time=10, index=3, level=0.3)
]
# Define initial condition and reference at t=0
ic = np.vstack((0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0))
ref0 = np.vstack((1, -1, -2, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0))
# Define agents
self.plant = Copter_nonlinear()
self.n = self.plant.mixer.B.shape[1]
self.fdi = FDI(numact=self.n)
self.controller = AdaptiveISMC_nonlinear(self.plant.J,
self.plant.m,
self.plant.g,
self.plant.d,
ic,
ref0)（2）函數(shù)Env.get_ref(t, x)的功能是根據(jù)當前時間 t 和狀態(tài) x 計算參考軌跡 ref，包括期望位置、速度、姿態(tài)和角速度。
def get_ref(self, t, x):
pos_des = np.vstack((1, -1, -2))
vel_des = np.vstack((0, 0, 0))
quat_des = np.vstack((1, 0, 0, 0))
omega_des = np.zeros((3, 1))
ref = np.vstack((pos_des, vel_des, quat_des, omega_des))
return ref（3）函數(shù)Env._get_derivs(t, x, p, gamma, effectiveness)的功能根據(jù)當前時間 t、狀態(tài) x、控制參數(shù) p、滑模變量 gamma 和執(zhí)行器效應(yīng)性 effectiveness 計算執(zhí)行器指令、執(zhí)行器輸出、作用在飛行器上的力和滑模變量。
def _get_derivs(self, t, x, p, gamma, effectiveness):
ref = self.get_ref(t, x)
forces, sliding = self.controller.get_FM(x, ref, p, gamma, t)
# Controller
Bf = self.plant.mixer.B * effectiveness
L = np.diag(effectiveness)
rotors_cmd = np.linalg.pinv(Bf.dot(L)).dot(forces)
rotors = np.clip(rotors_cmd, 0, self.plant.rotor_max)
return rotors_cmd, rotors, forces, ref, sliding（4）函數(shù)Env.set_dot(t的功能根據(jù)當前時間 t 更新仿真狀態(tài)的導(dǎo)數(shù)，包括計算執(zhí)行器指令、執(zhí)行器輸出和作用在飛行器上的力，并更新控制器和飛行器的狀態(tài)。
def set_dot(self, t):
x = self.plant.state
p, gamma = self.controller.observe_list()
effectiveness = np.array([1.] * self.n)
for act_fault in self.actuator_faults:
effectiveness = act_fault.get_effectiveness(t, effectiveness)
rotors_cmd, rotors, forces, ref, sliding = \
self._get_derivs(t, x, p, gamma, effectiveness)
self.plant.set_dot(t, rotors)
self.controller.set_dot(x, ref, sliding)（5）函數(shù)Env.logger_callback(i, t, y, *args)是仿真過程中的日志記錄回調(diào)函數(shù)，用于提取并返回包括時間 t、飛行器狀態(tài) x、執(zhí)行器指令、執(zhí)行器輸出、參考軌跡、滑模變量和作用在飛行器上的力等信息的字典。
def logger_callback(self, i, t, y, *args):
states = self.observe_dict(y)
x_flat = self.plant.observe_vec(y[self.plant.flat_index])
ctrl_flat = self.controller.observe_list(y[self.controller.flat_index])
x = states["plant"]
effectiveness = np.array([1.] * self.n)
for act_fault in self.actuator_faults:
effectiveness = act_fault.get_effectiveness(t, effectiveness)
rotors_cmd, rotors, forces, ref, sliding = \
self._get_derivs(t, x_flat, ctrl_flat[0], ctrl_flat[1], effectiveness)
return dict(t=t, x=x, rotors=rotors, rotors_cmd=rotors_cmd,
ref=ref, gamma=ctrl_flat[1], forces=forces)（6）函數(shù)exp1_plot()用于繪制仿真結(jié)果的多個子圖，包括飛行器執(zhí)行器指令與實際輸出的對比、期望軌跡與實際軌跡的對比、執(zhí)行器故障檢測與隔離滑模變量的演變，以及在執(zhí)行器故障發(fā)生時的標注。通過可視化展示飛行器的動態(tài)響應(yīng)和控制性能，有助于分析控制系統(tǒng)的穩(wěn)定性和魯棒性。
def exp1_plot():
data = fym.logging.load("case3_A.h5")
print(data.keys())
# Rotor
plt.figure()
ax = plt.subplot(411)
plt.plot(data["t"], data["rotors"][:, 0], "k-", label="Response")
plt.plot(data["t"], data["rotors_cmd"][:, 0], "r--", label="Command")
plt.ylim([-5.1, 45])
plt.legend()
plt.subplot(412, sharex=ax)
plt.plot(data["t"], data["rotors"][:, 1], "k-")
plt.plot(data["t"], data["rotors_cmd"][:, 1], "r--")
plt.ylim([-5.1, 45])
plt.subplot(413, sharex=ax)
plt.plot(data["t"], data["rotors"][:, 2], "k-")
plt.plot(data["t"], data["rotors_cmd"][:, 2], "r--")
plt.ylim([-5.1, 45])
plt.subplot(414, sharex=ax)
plt.plot(data["t"], data["rotors"][:, 3], "k-")
plt.plot(data["t"], data["rotors_cmd"][:, 3], "r--")
plt.ylim([-5.1, 45])
plt.gcf().supxlabel("Time, sec")
plt.gcf().supylabel("Rotor force")
plt.tight_layout()
plt.figure()
ax = plt.subplot(411)
plt.plot(data["t"], data["ref"][:, 0, 0], "r-", label="x (cmd)")
plt.plot(data["t"], data["x"]["pos"][:, 0, 0], label="x")
plt.plot(data["t"], data["ref"][:, 1, 0], "r--", label="y (cmd)")
plt.plot(data["t"], data["x"]["pos"][:, 1, 0], label="y")
plt.plot(data["t"], data["ref"][:, 2, 0], "r-.", label="z (cmd)")
plt.plot(data["t"], data["x"]["pos"][:, 2, 0], label="z")
plt.legend(loc='center left', bbox_to_anchor=(1, 0.5))
plt.axvspan(5, 5.042, alpha=0.2, color="b")
plt.axvline(5.042, alpha=0.8, color="b", linewidth=0.5)
plt.annotate("Rotor 0 fails", xy=(5, 0), xytext=(5.5, 0.5),
arrowprops=dict(arrowstyle='->', lw=1.5))
plt.subplot(412, sharex=ax)
plt.plot(data["t"], data["x"]["vel"].squeeze())
plt.legend([r'$u$', r'$v$', r'$w$'], loc='center left', bbox_to_anchor=(1, 0.5))
plt.axvspan(5, 5.042, alpha=0.2, color="b")
plt.axvline(5.042, alpha=0.8, color="b", linewidth=0.5)
plt.annotate("Rotor 0 fails", xy=(5, 0), xytext=(5.5, 0.5),
arrowprops=dict(arrowstyle='->', lw=1.5))
plt.subplot(413, sharex=ax)
plt.plot(data["t"], data["x"]["quat"].squeeze())
plt.legend([r'$q0$', r'$q1$', r'$q2$', r'$q3$'])
# plt.plot(data["t"], np.transpose(quat2angle(np.transpose(data["x"]["quat"].squeeze()))))
# plt.legend([r'$psi$', r'$theta$', r'$phi$'], loc='center left', bbox_to_anchor=(1, 0.5))
plt.axvspan(5, 5.042, alpha=0.2, color="b")
plt.axvline(5.042, alpha=0.8, color="b", linewidth=0.5)
plt.annotate("Rotor 0 fails", xy=(5, 0), xytext=(5.5, 0.5),
arrowprops=dict(arrowstyle='->', lw=1.5))
plt.subplot(414, sharex=ax)
plt.plot(data["t"], data["x"]["omega"].squeeze())
plt.legend([r'$p$', r'$q$', r'$r$'], loc='center left', bbox_to_anchor=(1, 0.5))
plt.axvspan(5, 5.042, alpha=0.2, color="b")
plt.axvline(5.042, alpha=0.8, color="b", linewidth=0.5)
plt.annotate("Rotor 0 fails", xy=(5, 0), xytext=(5.5, 0.5),
arrowprops=dict(arrowstyle='->', lw=1.5))
# plt.xlabel("Time, sec")
# plt.ylabel("Position")
# plt.legend(loc="right")
plt.tight_layout()
plt.figure()
ax = plt.subplot(411)
plt.plot(data['t'], data['gamma'].squeeze()[:, 0])
plt.ylabel(r'$-\hat{h_{1}}$')
plt.grid(True)
plt.axvspan(5, 5.042, alpha=0.2, color="b")
plt.axvline(5.042, alpha=0.8, color="b", linewidth=0.5)
plt.annotate("Rotor 0 fails", xy=(5, 0), xytext=(5.5, 0.5),
arrowprops=dict(arrowstyle='->', lw=1.5))
plt.subplot(412, sharex=ax)
plt.plot(data['t'], data['gamma'].squeeze()[:, 1])
plt.ylabel(r'$\hat{h_{2}}$')
plt.grid(True)
plt.axvspan(5, 5.042, alpha=0.2, color="b")
plt.axvline(5.042, alpha=0.8, color="b", linewidth=0.5)
plt.annotate("Rotor 0 fails", xy=(5, 0), xytext=(5.5, 0.5),
arrowprops=dict(arrowstyle='->', lw=1.5))
plt.subplot(413, sharex=ax)
plt.plot(data['t'], data['gamma'].squeeze()[:, 2])
plt.ylabel(r'$\hat{h_{3}}$')
plt.grid(True)
plt.axvspan(5, 5.042, alpha=0.2, color="b")
plt.axvline(5.042, alpha=0.8, color="b", linewidth=0.5)
plt.annotate("Rotor 0 fails", xy=(5, 0), xytext=(5.5, 0.5),
arrowprops=dict(arrowstyle='->', lw=1.5))
plt.subplot(414, sharex=ax)
plt.plot(data['t'], data['gamma'].squeeze()[:, 3])
plt.ylabel(r'$\hat{h_{4}}$')
plt.grid(True)
plt.axvspan(5, 5.042, alpha=0.2, color="b")
plt.axvline(5.042, alpha=0.8, color="b", linewidth=0.5)
plt.annotate("Rotor 0 fails", xy=(5, 0), xytext=(5.5, 0.5),
arrowprops=dict(arrowstyle='->', lw=1.5))
plt.xlabel("Time [sec]")
plt.tight_layout()
# fig4 = plt.figure()
# ax1 = fig4.add_subplot(4, 1, 1)
# ax2 = fig4.add_subplot(4, 1, 2, sharex=ax1)
# ax3 = fig4.add_subplot(4, 1, 3, sharex=ax1)
# ax4 = fig4.add_subplot(4, 1, 4, sharex=ax1)
# ax1.plot(data['t'], data['forces'].squeeze()[:, 0])
# ax2.plot(data['t'], data['forces'].squeeze()[:, 1])
# ax3.plot(data['t'], data['forces'].squeeze()[:, 2])
# ax4.plot(data['t'], data['forces'].squeeze()[:, 3])
# ax1.set_ylabel('F')
# ax1.grid(True)
# ax2.set_ylabel('M1')
# ax2.grid(True)
# ax3.set_ylabel('M2')
# ax3.grid(True)
# ax4.set_ylabel('M3')
# ax4.grid(True)
# ax4.set_xlabel('Time [sec]')
# plt.tight_layout()
plt.show()執(zhí)行后將回執(zhí)多個子圖，如圖2-8所示。
- 執(zhí)行器輸出與指令對比圖：包含四個子圖，每個子圖對應(yīng)飛行器的一個執(zhí)行器，展示了執(zhí)行器的實際輸出與控制器指令之間的對比，以評估執(zhí)行器的性能。
- 控制系統(tǒng)狀態(tài)與期望軌跡對比圖：包含四個子圖，分別展示了飛行器的位置、速度、姿態(tài)以及角速度的實際狀態(tài)與期望軌跡之間的對比，以評估控制系統(tǒng)的跟蹤性能。
- 執(zhí)行器故障檢測與隔離滑模變量演變圖： 展示了執(zhí)行器故障檢測與隔離系統(tǒng)的滑模變量在時間上的演變，以分析故障檢測與隔離性能。
文章來源：http://www.zghlxwxcb.cn/news/detail-853341.html
圖2-9??繪制的子圖文章來源地址http://www.zghlxwxcb.cn/news/detail-853341.html
未完待續(xù)
到了這里，關(guān)于（2-3-3）位置控制算法：無人機運動控制系統(tǒng)——基于自適應(yīng)反演滑?？刂破鞯姆抡鏈y試的文章就介紹完了。如果您還想了解更多內(nèi)容，請在右上角搜索TOY模板網(wǎng)以前的文章或繼續(xù)瀏覽下面的相關(guān)文章，希望大家以后多多支持TOY模板網(wǎng)！
