🔬 Amaliyot: Quadcopter Simulyatsiyasi
Laboratoriya maqsadi
Bu laboratoriyada quadcopter dinamikasini simulyatsiya qilamiz. Parvoz kontrollerini ishlab chiqamiz va turli senariylarda dronni boshqarishni o'rganamiz.
Kerakli kutubxonalar
pip install numpy matplotlib scipy
1-laboratoriya: Quadcopter Dinamikasi
To'liq fizik model
import numpy as np
import matplotlib.pyplot as plt
from scipy.integrate import solve_ivp
from mpl_toolkits.mplot3d import Axes3D
class Quadcopter:
"""
Quadcopter fizik modeli
State vektor: [x, y, z, vx, vy, vz, phi, theta, psi, p, q, r]
- (x, y, z): pozitsiya
- (vx, vy, vz): tezlik
- (phi, theta, psi): Euler burchaklari (roll, pitch, yaw)
- (p, q, r): burchak tezliklari
"""
def __init__(self):
# Fizik parametrlar
self.m = 1.0 # massa (kg)
self.g = 9.81 # gravitatsiya
self.L = 0.25 # markaz-motor masofa (m)
self.Ixx = 0.01 # inersiya momenti x
self.Iyy = 0.01 # inersiya momenti y
self.Izz = 0.02 # inersiya momenti z
self.k = 2.5e-6 # lift konstantasi
self.b = 1e-7 # drag konstantasi
# Motor chegaralari
self.omega_min = 0
self.omega_max = 8000 # RPM
# State
self.state = np.zeros(12)
def set_initial_state(self, pos=[0, 0, 0], vel=[0, 0, 0],
angles=[0, 0, 0], ang_vel=[0, 0, 0]):
"""Boshlang'ich holatni o'rnatish"""
self.state = np.array([
*pos, *vel, *angles, *ang_vel
], dtype=float)
def rotation_matrix(self, phi, theta, psi):
"""Body -> Inertial rotation matritsasi"""
cp, sp = np.cos(phi), np.sin(phi)
ct, st = np.cos(theta), np.sin(theta)
cy, sy = np.cos(psi), np.sin(psi)
R = np.array([
[ct*cy, sp*st*cy - cp*sy, cp*st*cy + sp*sy],
[ct*sy, sp*st*sy + cp*cy, cp*st*sy - sp*cy],
[-st, sp*ct, cp*ct]
])
return R
def motor_forces(self, omega):
"""
Motor kuchlarini hisoblash
omega: [omega1, omega2, omega3, omega4] - motor tezliklari (rad/s)
Motor joylashuvi:
1
|
2 ---+--- 4
|
3
"""
# Umumiy tortish kuchi
T = self.k * np.sum(omega**2)
# Momentlar
tau_phi = self.L * self.k * (omega[1]**2 - omega[3]**2) # roll
tau_theta = self.L * self.k * (omega[0]**2 - omega[2]**2) # pitch
tau_psi = self.b * (omega[0]**2 - omega[1]**2 + omega[2]**2 - omega[3]**2) # yaw
return T, tau_phi, tau_theta, tau_psi
def dynamics(self, t, state, omega):
"""
Harakat tenglamalari
"""
x, y, z, vx, vy, vz, phi, theta, psi, p, q, r = state
# Motor kuchlari
T, tau_phi, tau_theta, tau_psi = self.motor_forces(omega)
# Rotation matrix
R = self.rotation_matrix(phi, theta, psi)
# Tortish vektori (body frame)
thrust_body = np.array([0, 0, T])
# Tortish (inertial frame)
thrust_inertial = R @ thrust_body
# Translatsion dinamika
ax = thrust_inertial[0] / self.m
ay = thrust_inertial[1] / self.m
az = thrust_inertial[2] / self.m - self.g
# Euler burchak tezliklari
# phi_dot, theta_dot, psi_dot = f(p, q, r)
phi_dot = p + np.sin(phi) * np.tan(theta) * q + np.cos(phi) * np.tan(theta) * r
theta_dot = np.cos(phi) * q - np.sin(phi) * r
psi_dot = (np.sin(phi) / np.cos(theta)) * q + (np.cos(phi) / np.cos(theta)) * r
# Burchak tezlanishlari (Euler tenglamalari)
p_dot = (tau_phi - (self.Izz - self.Iyy) * q * r) / self.Ixx
q_dot = (tau_theta - (self.Ixx - self.Izz) * p * r) / self.Iyy
r_dot = (tau_psi - (self.Iyy - self.Ixx) * p * q) / self.Izz
return np.array([vx, vy, vz, ax, ay, az, phi_dot, theta_dot, psi_dot, p_dot, q_dot, r_dot])
def hover_omega(self):
"""Hovering uchun motor tezligi"""
T_hover = self.m * self.g
omega_single = np.sqrt(T_hover / (4 * self.k))
return np.array([omega_single] * 4)
class QuadcopterController:
"""PID asosidagi parvoz kontrolleri"""
def __init__(self, quad):
self.quad = quad
# Pozitsiya PID
self.Kp_pos = np.array([2.0, 2.0, 4.0])
self.Ki_pos = np.array([0.1, 0.1, 0.2])
self.Kd_pos = np.array([3.0, 3.0, 5.0])
# Burchak PID
self.Kp_ang = np.array([8.0, 8.0, 4.0])
self.Ki_ang = np.array([0.0, 0.0, 0.0])
self.Kd_ang = np.array([4.0, 4.0, 2.0])
# Integral terms
self.pos_integral = np.zeros(3)
self.ang_integral = np.zeros(3)
# Oldingi xato
self.prev_pos_error = np.zeros(3)
self.prev_ang_error = np.zeros(3)
def reset(self):
"""Kontrollerni reset qilish"""
self.pos_integral = np.zeros(3)
self.ang_integral = np.zeros(3)
self.prev_pos_error = np.zeros(3)
self.prev_ang_error = np.zeros(3)
def compute_control(self, state, target_pos, target_yaw=0, dt=0.01):
"""
Kontrol signallarini hisoblash
state: [x, y, z, vx, vy, vz, phi, theta, psi, p, q, r]
target_pos: [x_d, y_d, z_d]
target_yaw: psi_d
return: omega[4] - motor tezliklari
"""
x, y, z, vx, vy, vz, phi, theta, psi, p, q, r = state
pos = np.array([x, y, z])
vel = np.array([vx, vy, vz])
# Pozitsiya xatosi
pos_error = np.array(target_pos) - pos
# PID - pozitsiya
self.pos_integral += pos_error * dt
pos_derivative = (pos_error - self.prev_pos_error) / dt
self.prev_pos_error = pos_error.copy()
# Kerakli tezlanish
acc_desired = (self.Kp_pos * pos_error +
self.Ki_pos * self.pos_integral +
self.Kd_pos * pos_derivative)
# Kerakli tortish va burchaklar
T_desired = self.quad.m * (self.quad.g + acc_desired[2])
# Yaw rotation
psi_c = psi # Hozirgi yaw
# Kerakli roll va pitch
phi_d = (1 / self.quad.g) * (acc_desired[0] * np.sin(psi_c) -
acc_desired[1] * np.cos(psi_c))
theta_d = (1 / self.quad.g) * (acc_desired[0] * np.cos(psi_c) +
acc_desired[1] * np.sin(psi_c))
# Burchaklarni cheklash
max_angle = np.radians(30)
phi_d = np.clip(phi_d, -max_angle, max_angle)
theta_d = np.clip(theta_d, -max_angle, max_angle)
# Burchak xatosi
ang_error = np.array([phi_d - phi, theta_d - theta, target_yaw - psi])
# Yaw wrap
ang_error[2] = np.arctan2(np.sin(ang_error[2]), np.cos(ang_error[2]))
# PID - burchak
self.ang_integral += ang_error * dt
ang_derivative = np.array([p, q, r]) # Burchak tezliklari
# Kerakli momentlar
tau = (self.Kp_ang * ang_error +
self.Ki_ang * self.ang_integral -
self.Kd_ang * ang_derivative)
# Motor tezliklariga aylantirish
omega = self.allocation(T_desired, tau)
return omega
def allocation(self, T, tau):
"""
Tortish va momentlardan motor tezliklariga aylantirish
T: umumiy tortish
tau: [tau_phi, tau_theta, tau_psi] momentlar
"""
k = self.quad.k
b = self.quad.b
L = self.quad.L
# Allocation matrix
# [T] [k k k k ] [w1^2]
# [tau_phi] = [0 kL 0 -kL ] [w2^2]
# [tau_theta] [kL 0 -kL 0 ] [w3^2]
# [tau_psi] [b -b b -b ] [w4^2]
A = np.array([
[k, k, k, k],
[0, k*L, 0, -k*L],
[k*L, 0, -k*L, 0],
[b, -b, b, -b]
])
u = np.array([T, tau[0], tau[1], tau[2]])
omega_sq = np.linalg.solve(A, u)
omega_sq = np.maximum(omega_sq, 0) # Manfiy bo'lmasin
omega = np.sqrt(omega_sq)
# Motor chegaralari
omega = np.clip(omega, self.quad.omega_min, self.quad.omega_max)
return omega
def simulate_hover():
"""Hovering simulyatsiyasi"""
quad = Quadcopter()
controller = QuadcopterController(quad)
# Boshlang'ich holat
quad.set_initial_state(pos=[0, 0, 0], vel=[0, 0, 0])
# Simulyatsiya parametrlari
dt = 0.01
t_end = 10
t = np.arange(0, t_end, dt)
# Target
target_pos = [0, 0, 2] # 2m balandlikka ko'tarilish
# Ma'lumotlarni saqlash
states = []
omegas = []
state = quad.state.copy()
for i in range(len(t)):
# Kontrol
omega = controller.compute_control(state, target_pos, dt=dt)
omegas.append(omega.copy())
# Dinamika
def dynamics_wrapper(t_inner, s):
return quad.dynamics(t_inner, s, omega)
sol = solve_ivp(dynamics_wrapper, [0, dt], state, method='RK45')
state = sol.y[:, -1]
# Ground constraint
if state[2] < 0:
state[2] = 0
state[5] = max(0, state[5])
states.append(state.copy())
states = np.array(states)
omegas = np.array(omegas)
# Vizualizatsiya
fig, axes = plt.subplots(3, 2, figsize=(14, 10))
# Pozitsiya
axes[0, 0].plot(t, states[:, 0], 'r-', label='x')
axes[0, 0].plot(t, states[:, 1], 'g-', label='y')
axes[0, 0].plot(t, states[:, 2], 'b-', label='z')
axes[0, 0].axhline(y=target_pos[2], color='b', linestyle='--', alpha=0.5)
axes[0, 0].set_ylabel('Pozitsiya (m)')
axes[0, 0].set_title('Pozitsiya')
axes[0, 0].legend()
axes[0, 0].grid(True, alpha=0.3)
# Tezlik
axes[0, 1].plot(t, states[:, 3], 'r-', label='vx')
axes[0, 1].plot(t, states[:, 4], 'g-', label='vy')
axes[0, 1].plot(t, states[:, 5], 'b-', label='vz')
axes[0, 1].set_ylabel('Tezlik (m/s)')
axes[0, 1].set_title('Tezlik')
axes[0, 1].legend()
axes[0, 1].grid(True, alpha=0.3)
# Burchaklar
axes[1, 0].plot(t, np.degrees(states[:, 6]), 'r-', label='roll (φ)')
axes[1, 0].plot(t, np.degrees(states[:, 7]), 'g-', label='pitch (θ)')
axes[1, 0].plot(t, np.degrees(states[:, 8]), 'b-', label='yaw (ψ)')
axes[1, 0].set_ylabel('Burchak (°)')
axes[1, 0].set_title('Euler Burchaklari')
axes[1, 0].legend()
axes[1, 0].grid(True, alpha=0.3)
# Burchak tezliklari
axes[1, 1].plot(t, np.degrees(states[:, 9]), 'r-', label='p')
axes[1, 1].plot(t, np.degrees(states[:, 10]), 'g-', label='q')
axes[1, 1].plot(t, np.degrees(states[:, 11]), 'b-', label='r')
axes[1, 1].set_ylabel('Burchak tezligi (°/s)')
axes[1, 1].set_title('Burchak Tezliklari')
axes[1, 1].legend()
axes[1, 1].grid(True, alpha=0.3)
# Motor tezliklari
axes[2, 0].plot(t, omegas[:, 0], label='ω₁')
axes[2, 0].plot(t, omegas[:, 1], label='ω₂')
axes[2, 0].plot(t, omegas[:, 2], label='ω₃')
axes[2, 0].plot(t, omegas[:, 3], label='ω₄')
axes[2, 0].set_xlabel('Vaqt (s)')
axes[2, 0].set_ylabel('Motor tezligi (rad/s)')
axes[2, 0].set_title('Motor Tezliklari')
axes[2, 0].legend()
axes[2, 0].grid(True, alpha=0.3)
# 3D trajectory
ax3d = fig.add_subplot(3, 2, 6, projection='3d')
ax3d.plot(states[:, 0], states[:, 1], states[:, 2], 'b-', linewidth=2)
ax3d.scatter(*target_pos, c='red', s=100, marker='*', label='Target')
ax3d.scatter(0, 0, 0, c='green', s=100, marker='o', label='Start')
ax3d.set_xlabel('X (m)')
ax3d.set_ylabel('Y (m)')
ax3d.set_zlabel('Z (m)')
ax3d.set_title('3D Trajectory')
ax3d.legend()
plt.tight_layout()
plt.show()
if __name__ == "__main__":
print("="*50)
print("QUADCOPTER HOVERING SIMULYATSIYASI")
print("="*50)
simulate_hover()
2-laboratoriya: Waypoint Following
import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
from scipy.integrate import solve_ivp
# Oldingi Quadcopter va Controller classlarini import qiling
def simulate_waypoint_mission():
"""Waypoint missiyasi simulyatsiyasi"""
quad = Quadcopter()
controller = QuadcopterController(quad)
# Boshlang'ich holat
quad.set_initial_state(pos=[0, 0, 0])
# Waypoints
waypoints = [
[0, 0, 2], # Takeoff
[3, 0, 2], # Forward
[3, 3, 2], # Right
[0, 3, 3], # Back and up
[0, 0, 3], # Return
[0, 0, 1], # Descend
]
dt = 0.01
t_total = 0
all_states = []
all_times = []
state = quad.state.copy()
current_wp = 0
wp_threshold = 0.3 # Waypoint yetib borish aniqligi
while current_wp < len(waypoints):
target = waypoints[current_wp]
controller.reset()
# Bu waypoint ga yetguncha
max_time = 10 # Har bir waypoint uchun max vaqt
wp_time = 0
while wp_time < max_time:
omega = controller.compute_control(state, target, dt=dt)
def dynamics_wrapper(t_inner, s):
return quad.dynamics(t_inner, s, omega)
sol = solve_ivp(dynamics_wrapper, [0, dt], state, method='RK45')
state = sol.y[:, -1]
if state[2] < 0:
state[2] = 0
state[5] = max(0, state[5])
all_states.append(state.copy())
all_times.append(t_total)
t_total += dt
wp_time += dt
# Waypoint yetib borganmizni tekshirish
pos_error = np.linalg.norm(state[:3] - target)
if pos_error < wp_threshold:
print(f"Waypoint {current_wp + 1} reached at t = {t_total:.2f}s")
break
current_wp += 1
states = np.array(all_states)
times = np.array(all_times)
# 3D vizualizatsiya
fig = plt.figure(figsize=(14, 6))
ax1 = fig.add_subplot(121, projection='3d')
ax1.plot(states[:, 0], states[:, 1], states[:, 2], 'b-', linewidth=1, alpha=0.7)
# Waypoints
wps = np.array(waypoints)
ax1.scatter(wps[:, 0], wps[:, 1], wps[:, 2], c='red', s=200, marker='*', label='Waypoints')
# Waypoint raqamlari
for i, wp in enumerate(waypoints):
ax1.text(wp[0], wp[1], wp[2] + 0.3, f'WP{i+1}', fontsize=10, ha='center')
ax1.set_xlabel('X (m)')
ax1.set_ylabel('Y (m)')
ax1.set_zlabel('Z (m)')
ax1.set_title('Waypoint Mission', fontsize=14)
ax1.legend()
# 2D ko'rinishlar
ax2 = fig.add_subplot(122)
ax2.plot(states[:, 0], states[:, 1], 'b-', linewidth=1, label='Trajectory')
ax2.scatter(wps[:, 0], wps[:, 1], c='red', s=100, marker='*', label='Waypoints')
for i, wp in enumerate(waypoints):
ax2.annotate(f'WP{i+1}', (wp[0], wp[1]), textcoords="offset points",
xytext=(5, 5), fontsize=10)
ax2.set_xlabel('X (m)')
ax2.set_ylabel('Y (m)')
ax2.set_title('Top View (XY)', fontsize=14)
ax2.set_aspect('equal')
ax2.grid(True, alpha=0.3)
ax2.legend()
plt.tight_layout()
plt.show()
if __name__ == "__main__":
print("="*50)
print("WAYPOINT MISSION SIMULYATSIYASI")
print("="*50)
simulate_waypoint_mission()
3-laboratoriya: Wind Disturbance
import numpy as np
import matplotlib.pyplot as plt
from scipy.integrate import solve_ivp
class QuadcopterWithWind(Quadcopter):
"""Shamol ta'siri bilan quadcopter"""
def __init__(self):
super().__init__()
self.wind_velocity = np.array([0.0, 0.0, 0.0])
self.wind_gust_amplitude = 0.0
self.wind_gust_freq = 0.5
def set_wind(self, velocity, gust_amplitude=0):
"""Shamol parametrlarini o'rnatish"""
self.wind_velocity = np.array(velocity)
self.wind_gust_amplitude = gust_amplitude
def get_wind(self, t):
"""Hozirgi shamol tezligi"""
# Doimiy shamol + sinusoidal gust
gust = self.wind_gust_amplitude * np.sin(2 * np.pi * self.wind_gust_freq * t)
return self.wind_velocity + np.array([gust, 0, 0])
def dynamics_with_wind(self, t, state, omega):
"""Shamol bilan dinamika"""
x, y, z, vx, vy, vz, phi, theta, psi, p, q, r = state
# Asosiy dinamika
base_deriv = self.dynamics(t, state, omega)
# Shamol ta'siri
wind = self.get_wind(t)
relative_vel = np.array([vx, vy, vz]) - wind
# Drag kuchi (soddalashtirilgan)
Cd = 0.5 # drag koeffitsienti
A = 0.1 # cross-section area
rho = 1.225 # havo zichligi
drag = -0.5 * rho * Cd * A * np.linalg.norm(relative_vel) * relative_vel / self.m
# Drag ni qo'shish
base_deriv[3] += drag[0]
base_deriv[4] += drag[1]
base_deriv[5] += drag[2]
return base_deriv
def simulate_wind_disturbance():
"""Shamol buzilishi simulyatsiyasi"""
quad = QuadcopterWithWind()
controller = QuadcopterController(quad)
# Shamol o'rnatish
quad.set_wind(velocity=[2.0, 0.5, 0], gust_amplitude=1.5)
# Boshlang'ich holat
quad.set_initial_state(pos=[0, 0, 2])
dt = 0.01
t_end = 20
t = np.arange(0, t_end, dt)
target_pos = [0, 0, 2]
states = []
winds = []
state = quad.state.copy()
for i, ti in enumerate(t):
omega = controller.compute_control(state, target_pos, dt=dt)
def dynamics_wrapper(t_inner, s):
return quad.dynamics_with_wind(ti + t_inner, s, omega)
sol = solve_ivp(dynamics_wrapper, [0, dt], state, method='RK45')
state = sol.y[:, -1]
if state[2] < 0:
state[2] = 0
state[5] = max(0, state[5])
states.append(state.copy())
winds.append(quad.get_wind(ti).copy())
states = np.array(states)
winds = np.array(winds)
# Vizualizatsiya
fig, axes = plt.subplots(2, 2, figsize=(14, 10))
# Pozitsiya
axes[0, 0].plot(t, states[:, 0], 'r-', label='x')
axes[0, 0].plot(t, states[:, 1], 'g-', label='y')
axes[0, 0].plot(t, states[:, 2], 'b-', label='z')
axes[0, 0].axhline(y=0, color='k', linestyle='--', alpha=0.3)
axes[0, 0].axhline(y=2, color='b', linestyle='--', alpha=0.3, label='target z')
axes[0, 0].set_ylabel('Pozitsiya (m)')
axes[0, 0].set_title('Pozitsiya (Shamol Ta\'sirida)', fontsize=14)
axes[0, 0].legend()
axes[0, 0].grid(True, alpha=0.3)
# Shamol
axes[0, 1].plot(t, winds[:, 0], 'b-', label='Wind X')
axes[0, 1].plot(t, winds[:, 1], 'g-', label='Wind Y')
axes[0, 1].fill_between(t, winds[:, 0], alpha=0.3)
axes[0, 1].set_ylabel('Shamol tezligi (m/s)')
axes[0, 1].set_title('Shamol Profili', fontsize=14)
axes[0, 1].legend()
axes[0, 1].grid(True, alpha=0.3)
# XY trajectory
axes[1, 0].plot(states[:, 0], states[:, 1], 'b-', linewidth=1)
axes[1, 0].scatter(0, 0, c='red', s=100, marker='*', label='Target')
axes[1, 0].scatter(states[0, 0], states[0, 1], c='green', s=100, marker='o', label='Start')
axes[1, 0].set_xlabel('X (m)')
axes[1, 0].set_ylabel('Y (m)')
axes[1, 0].set_title('Trajectory (Top View)', fontsize=14)
axes[1, 0].set_aspect('equal')
axes[1, 0].legend()
axes[1, 0].grid(True, alpha=0.3)
# Burchaklar
axes[1, 1].plot(t, np.degrees(states[:, 6]), 'r-', label='roll')
axes[1, 1].plot(t, np.degrees(states[:, 7]), 'g-', label='pitch')
axes[1, 1].set_xlabel('Vaqt (s)')
axes[1, 1].set_ylabel('Burchak (°)')
axes[1, 1].set_title('Kompensatsiya Burchaklari', fontsize=14)
axes[1, 1].legend()
axes[1, 1].grid(True, alpha=0.3)
plt.tight_layout()
plt.show()
if __name__ == "__main__":
print("="*50)
print("SHAMOL BUZILISHI SIMULYATSIYASI")
print("="*50)
simulate_wind_disturbance()
4-laboratoriya: Altitude Hold Mode
import numpy as np
import matplotlib.pyplot as plt
class AltitudeHoldController:
"""Balandlik saqlash kontrolleri"""
def __init__(self, quad):
self.quad = quad
# Balandlik PID
self.Kp_z = 5.0
self.Ki_z = 0.5
self.Kd_z = 3.0
# Attitude PID (soddalashtirilgan)
self.Kp_att = 5.0
self.Kd_att = 2.0
self.z_integral = 0
self.prev_z_error = 0
def compute_control(self, state, target_z, roll_cmd=0, pitch_cmd=0, yaw_rate_cmd=0, dt=0.01):
"""
Altitude hold kontrol
roll_cmd, pitch_cmd: pilot buyruqlari (radians, -0.5 to 0.5)
yaw_rate_cmd: yaw rate buyrug'i (rad/s)
"""
z = state[2]
vz = state[5]
phi, theta, psi = state[6:9]
p, q, r = state[9:12]
# Balandlik PID
z_error = target_z - z
self.z_integral += z_error * dt
z_derivative = -vz # Tezlik
T_base = self.quad.m * self.quad.g # Hover thrust
T_correction = (self.Kp_z * z_error +
self.Ki_z * self.z_integral +
self.Kd_z * z_derivative)
# Burchak kompensatsiyasi (tilted thrust)
T = (T_base + T_correction) / (np.cos(phi) * np.cos(theta))
T = np.clip(T, 0, 2 * T_base) # Cheklash
# Attitude kontrol
phi_error = roll_cmd - phi
theta_error = pitch_cmd - theta
psi_rate_error = yaw_rate_cmd - r
tau_phi = self.Kp_att * phi_error - self.Kd_att * p
tau_theta = self.Kp_att * theta_error - self.Kd_att * q
tau_psi = self.Kp_att * psi_rate_error
tau = np.array([tau_phi, tau_theta, tau_psi])
# Motor allocation
omega = self._allocation(T, tau)
return omega
def _allocation(self, T, tau):
"""Motor allocation"""
k = self.quad.k
b = self.quad.b
L = self.quad.L
A = np.array([
[k, k, k, k],
[0, k*L, 0, -k*L],
[k*L, 0, -k*L, 0],
[b, -b, b, -b]
])
u = np.array([T, tau[0], tau[1], tau[2]])
omega_sq = np.linalg.solve(A, u)
omega_sq = np.maximum(omega_sq, 0)
omega = np.sqrt(omega_sq)
return np.clip(omega, 0, self.quad.omega_max)
def simulate_altitude_hold():
"""Altitude hold simulyatsiyasi"""
quad = Quadcopter()
controller = AltitudeHoldController(quad)
quad.set_initial_state(pos=[0, 0, 2])
dt = 0.01
t_end = 30
t = np.arange(0, t_end, dt)
states = []
commands = []
state = quad.state.copy()
target_z = 2.0
for i, ti in enumerate(t):
# Pilot buyruqlari (simulyatsiya)
roll_cmd = 0
pitch_cmd = 0
yaw_rate_cmd = 0
# 5-10s: oldinga parvoz
if 5 < ti < 10:
pitch_cmd = np.radians(10)
# 12-17s: o'ngga parvoz
if 12 < ti < 17:
roll_cmd = np.radians(-10)
# 20-25s: aylanish
if 20 < ti < 25:
yaw_rate_cmd = np.radians(30)
commands.append([roll_cmd, pitch_cmd, yaw_rate_cmd])
omega = controller.compute_control(state, target_z,
roll_cmd, pitch_cmd, yaw_rate_cmd, dt)
def dynamics_wrapper(t_inner, s):
return quad.dynamics(t_inner, s, omega)
from scipy.integrate import solve_ivp
sol = solve_ivp(dynamics_wrapper, [0, dt], state, method='RK45')
state = sol.y[:, -1]
if state[2] < 0:
state[2] = 0
state[5] = max(0, state[5])
states.append(state.copy())
states = np.array(states)
commands = np.array(commands)
# Vizualizatsiya
fig, axes = plt.subplots(2, 2, figsize=(14, 10))
# Balandlik
axes[0, 0].plot(t, states[:, 2], 'b-', linewidth=2, label='Actual Z')
axes[0, 0].axhline(y=target_z, color='r', linestyle='--', label='Target Z')
axes[0, 0].fill_between(t, target_z - 0.1, target_z + 0.1, alpha=0.2, color='green')
axes[0, 0].set_ylabel('Balandlik (m)')
axes[0, 0].set_title('Altitude Hold Performance', fontsize=14)
axes[0, 0].legend()
axes[0, 0].grid(True, alpha=0.3)
# XY trajectory
axes[0, 1].plot(states[:, 0], states[:, 1], 'b-', linewidth=1)
axes[0, 1].scatter(0, 0, c='green', s=100, marker='o', label='Start')
axes[0, 1].scatter(states[-1, 0], states[-1, 1], c='red', s=100, marker='*', label='End')
axes[0, 1].set_xlabel('X (m)')
axes[0, 1].set_ylabel('Y (m)')
axes[0, 1].set_title('Gorizontal Trajectory', fontsize=14)
axes[0, 1].set_aspect('equal')
axes[0, 1].legend()
axes[0, 1].grid(True, alpha=0.3)
# Buyruqlar
axes[1, 0].plot(t, np.degrees(commands[:, 0]), 'r-', label='Roll cmd')
axes[1, 0].plot(t, np.degrees(commands[:, 1]), 'g-', label='Pitch cmd')
axes[1, 0].plot(t, np.degrees(commands[:, 2]), 'b-', label='Yaw rate cmd')
axes[1, 0].set_xlabel('Vaqt (s)')
axes[1, 0].set_ylabel('Buyruq (°)')
axes[1, 0].set_title('Pilot Buyruqlari', fontsize=14)
axes[1, 0].legend()
axes[1, 0].grid(True, alpha=0.3)
# Actual burchaklar
axes[1, 1].plot(t, np.degrees(states[:, 6]), 'r-', label='Roll')
axes[1, 1].plot(t, np.degrees(states[:, 7]), 'g-', label='Pitch')
axes[1, 1].plot(t, np.degrees(states[:, 8]), 'b-', label='Yaw')
axes[1, 1].set_xlabel('Vaqt (s)')
axes[1, 1].set_ylabel('Burchak (°)')
axes[1, 1].set_title('Actual Burchaklar', fontsize=14)
axes[1, 1].legend()
axes[1, 1].grid(True, alpha=0.3)
plt.tight_layout()
plt.show()
if __name__ == "__main__":
print("="*50)
print("ALTITUDE HOLD MODE SIMULYATSIYASI")
print("="*50)
simulate_altitude_hold()
Xulosa
Bu laboratoriyalarda biz:
- ✅ Quadcopter fizik modelini yaratdik
- ✅ PID kontrollerini ishlab chiqdik
- ✅ Waypoint following ni amalga oshirdik
- ✅ Shamol ta'sirini simulyatsiya qildik
- ✅ Altitude hold rejimini yaratdik
Keyingi qadamlar
- GPS waypoint navigation
- Obstacle avoidance
- Formation flying
- Real hardware (ArduPilot/PX4) bilan ishlash