三自由度火箭运动轨迹仿真与姿态控制程序代码
时间: 2023-10-08 09:09:31 浏览: 96
飞行器控制程序代码
以下是一个简单的三自由度火箭运动轨迹仿真与姿态控制程序代码的示例:
```python
import numpy as np
import matplotlib.pyplot as plt
# 常数定义
g = 9.8 # 重力加速度
m = 1.0 # 火箭质量
c = 10.0 # 推进器推力系数
I = np.diag([0.01, 0.01, 0.02]) # 火箭转动惯量
l = 0.5 # 火箭长度
dt = 0.01 # 时间间隔
# 初始状态定义
x0 = np.array([0.0, 0.0, 0.0, 0.0, 0.0, np.pi/6, 0.0, 0.0, 0.0]) # [x, y, z, u, v, w, θ, φ, ψ]
u0 = np.array([m*g, 0.0, 0.0, 0.0]) # [F, τx, τy, τz]
# 轨迹模拟
def simulate(x0, u0):
x = x0.copy()
u = u0.copy()
traj = [x]
for i in range(1000):
F = u[0] - c*np.linalg.norm(x[3:6])*np.sin(x[6])
τ = u[1:]
xdot = np.zeros(9)
xdot[0:3] = x[3:6]
xdot[3:6] = np.array([0.0, 0.0, -g]) + np.array([1/m, 0.0, 0.0])*F + np.dot(get_rotation_matrix(x[6], x[7], x[8]), τ)/m
xdot[6:9] = np.dot(np.linalg.inv(get_rotation_matrix(x[6], x[7], x[8])), x[3:6]/l) - np.array([x[7], x[8], 0.0])
x = x + dt * xdot
u = np.array([F, τ[0], τ[1], τ[2]])
traj.append(x)
return np.array(traj)
# 轨迹可视化
traj = simulate(x0, u0)
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
ax.plot(traj[:, 0], traj[:, 1], traj[:, 2])
ax.set_xlabel('x')
ax.set_ylabel('y')
ax.set_zlabel('z')
plt.show()
# 姿态控制
def control(x):
Kp = np.diag([1.0, 1.0, 1.0])
Kd = np.diag([0.1, 0.1, 0.1])
θ_desired = np.arctan2(-x[0], l)
φ_desired = np.arctan2(-x[1], l)
ψ_desired = 0.0
r_desired = np.array([θ_desired, φ_desired, ψ_desired])
rdot_desired = np.zeros(3)
rddot_desired = np.zeros(3)
τ = np.dot(Kp, r_desired - np.array([x[6], x[7], x[8]])) - np.dot(Kd, np.array([x[3], x[4], x[5]]) - rdot_desired)
return np.array([m*g, τ[0], τ[1], τ[2]])
# 带控制的轨迹模拟
def simulate_controlled(x0):
x = x0.copy()
traj = [x]
for i in range(1000):
u = control(x)
F = u[0] - c*np.linalg.norm(x[3:6])*np.sin(x[6])
τ = u[1:]
xdot = np.zeros(9)
xdot[0:3] = x[3:6]
xdot[3:6] = np.array([0.0, 0.0, -g]) + np.array([1/m, 0.0, 0.0])*F + np.dot(get_rotation_matrix(x[6], x[7], x[8]), τ)/m
xdot[6:9] = np.dot(np.linalg.inv(get_rotation_matrix(x[6], x[7], x[8])), x[3:6]/l) - np.array([x[7], x[8], 0.0])
x = x + dt * xdot
traj.append(x)
return np.array(traj)
# 带控制的轨迹可视化
traj = simulate_controlled(x0)
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
ax.plot(traj[:, 0], traj[:, 1], traj[:, 2])
ax.set_xlabel('x')
ax.set_ylabel('y')
ax.set_zlabel('z')
plt.show()
```
这个示例程序使用欧拉法对三自由度火箭的运动轨迹进行了简单的仿真,并实现了一个简单的姿态控制器。请注意,这只是一个示例代码,实际应用中需要根据具体情况进行修改和优化。
阅读全文