1. 导入需要的库
import numpy as np
import csv
2. 定义三角函数用于坐标系转换
# cos(x)
def C(x):
return np.cos(x)
# sin(x)
def S(x):
return np.sin(x)
3. 定义转换矩阵：大地坐标系---->随体坐标系
def earth_to_body_frame(ii, jj, kk):
# C^b_n
R = [[C(kk) * C(jj), C(kk) * S(jj) * S(ii) - S(kk) * C(ii), C(kk) * S(jj) * C(ii) + S(kk) * S(ii)],
[S(kk) * C(jj), S(kk) * S(jj) * S(ii) + C(kk) * C(ii), S(kk) * S(jj) * C(ii) - C(kk) * S(ii)],
[-S(jj), C(jj) * S(ii), C(jj) * C(ii)]]
return np.array(R)
4. 定义转换矩阵：随体坐标系---->大地坐标系
def body_to_earth_frame(ii, jj, kk):
# C^n_b
return np.transpose(earth_to_body_frame(ii, jj, kk))
5. 将飞行器运动模型定义成一个类
5.1 定义__init__()函数
class PhysicsSim():
def __init__(self, init_pose=None, init_velocities=None, init_angle_velocities=None, runtime=5.):
self.init_pose = init_pose
self.init_velocities = init_velocities
self.init_angle_velocities = init_angle_velocities
self.runtime = runtime
self.gravity = -9.81 # m/s
self.rho = 1.2 # 油门线性占空比
self.mass = 0.958 # 300 g
self.dt = 1 / 50.0 # Timestep
self.C_d = 0.3 # 螺旋桨拉力系数
self.l_to_rotor = 0.4
self.propeller_size = 0.1 # 螺旋桨尺寸
width, length, height = .51, .51, .235 # 飞机的尺寸规格
self.dims = np.array([width, length, height]) # x, y, z dimensions of quadcopter
self.areas = np.array([length * height, width * height, width * length])
# 转动惯量
I_x = 1 / 12. * self.mass * (height**2 + width**2)
I_y = 1 / 12. * self.mass * (height**2 + length**2) # 0.0112 was a measured value
I_z = 1 / 12. * self.mass * (width**2 + length**2)
self.moments_of_inertia = np.array([I_x, I_y, I_z]) # 惯性矩
# 限定飞行器的运动范围
env_bounds = 300.0 # 300 m / 300 m / 300 m
self.lower_bounds = np.array([-env_bounds / 2, -env_bounds / 2, 0])
# [-150, -150, 0]
self.upper_bounds = np.array([env_bounds / 2, env_bounds / 2, env_bounds])
# [150,150,300]
self.reset()
5.2 定义reset(self)函数
与最后的
next_step()函数对应，这里出现的变量，在next_step()函数中都会再出现
- 时间
- 位姿：
初始化为[0, 0, 10, 0, 0, 0]
- 对地速度：
初始化为[0, 0, 0]
- 对地角速度：
初始化为[0, 0, 0]
- 对地加速度
- 对地角加速度
- 螺旋桨风速
def reset(self):
1. self.time = 0.0
2. self.pose = np.array([0.0, 0.0, 10.0, 0.0, 0.0, 0.0]) if self.init_pose is None else self.init_pose
3. self.v = np.array([0.0, 0.0, 0.0]) if self.init_velocities is None else self.init_velocities
4. self.angular_v = np.array([0.0, 0.0, 0.0]) if self.init_angle_velocities is None else self.init_angle_velocities
5. self.linear_accel = np.array([0.0, 0.0, 0.0])
6. self.angular_accels = np.array([0.0, 0.0, 0.0])
7. self.prop_wind_speed = np.array([0., 0., 0., 0.])
8. self.done = False
5.3 计算随体速度
调用
earth_to_body_frame(ii, jj, kk)转换矩阵：大地坐标系---->随体坐标系
返回随体速度
def find_body_velocity(self):
body_velocity = np.matmul(earth_to_body_frame(*list(self.pose[3:])), self.v)
return body_velocity
5.4 计算阻力
def get_linear_drag(self):
linear_drag = 0.5 * self.rho * self.find_body_velocity()**2 * self.areas * self.C_d
return linear_drag
5.5 计算螺旋桨产生的拉力
def get_linear_forces(self, thrusts):
# Gravity
gravity_force = self.mass * self.gravity * np.array([0, 0, 1])
# Thrust
thrust_body_force = np.array([0, 0, sum(thrusts)])
# Drag
drag_body_force = -self.get_linear_drag()
body_forces = thrust_body_force + drag_body_force
linear_forces = np.matmul(body_to_earth_frame(*list(self.pose[3:])), body_forces)
linear_forces += gravity_force
return linear_forces
5.6 计算力矩
def get_moments(self, thrusts):
# 推力矩
thrust_moment = np.array([(thrusts[3] - thrusts[2]) * self.l_to_rotor,
(thrusts[1] - thrusts[0]) * self.l_to_rotor,
0])# (thrusts[2] + thrusts[3] - thrusts[0] - thrusts[1]) * self.T_q]) # Moment from thrust
drag_moment = self.C_d * 0.5 * self.rho * self.angular_v * np.absolute(self.angular_v) * self.areas * self.dims * self.dims
moments = thrust_moment - drag_moment # + motor_inertia_moment
return moments
5.7 计算螺旋桨风速
def calc_prop_wind_speed(self):
body_velocity = self.find_body_velocity()
phi_dot, theta_dot = self.angular_v[0], self.angular_v[1]
s_0 = np.array([0., 0., theta_dot * self.l_to_rotor])
s_1 = -s_0
s_2 = np.array([0., 0., phi_dot * self.l_to_rotor])
s_3 = -s_2
speeds = [s_0, s_1, s_2, s_3]
for num in range(4):
perpendicular_speed = speeds[num] + body_velocity
self.prop_wind_speed[num] = perpendicular_speed[2]
5.8 计算净推力 - thrusts
def get_propeler_thrust(self, rotor_speeds):
'''根据螺旋桨的速度和输入功率计算净推力（推力 - 阻力）'''
thrusts = []
for prop_number in range(4):
V = self.prop_wind_speed[prop_number]
D = self.propeller_size
n = rotor_speeds[prop_number]
J = V / n * D
# From http://m-selig.ae.illinois.edu/pubs/BrandtSelig-2011-AIAA-2011-1255-LRN-Propellers.pdf
C_T = max(.12 - .07*max(0, J)-.1*max(0, J)**2, 0)
thrusts.append(C_T * self.rho * n**2 * D**4)
return thrusts
5.9 计算下一时间步的状态
此处使用的是前向欧拉方程：从当前时刻出发，根据当前时刻的函数值及其导数，可得到下一时刻的值
参考：//www.greatytc.com/p/e774e75f1263
def next_timestep(self, rotor_speeds):
7. self.calc_prop_wind_speed()
thrusts = self.get_propeler_thrust(rotor_speeds)
5. self.linear_accel = self.get_linear_forces(thrusts) / self.mass
position = self.pose[:3] + self.v * self.dt + 0.5 * self.linear_accel * self.dt**2
3. self.v += self.linear_accel * self.dt
moments = self.get_moments(thrusts)
6. self.angular_accels = moments / self.moments_of_inertia
angles = self.pose[3:] + self.angular_v * self.dt + 0.5 * self.angular_accels* self.dt*self.dt
angles = (angles + 2 * np.pi) % (2 * np.pi)
4. self.angular_v = self.angular_v + self.angular_accels * self.dt
new_positions = []
for ii in range(3):
if position[ii] <= self.lower_bounds[ii]:
new_positions.append(self.lower_bounds[ii])
self.done = True
elif position[ii] > self.upper_bounds[ii]:
new_positions.append(self.upper_bounds[ii])
self.done = True
else:
new_positions.append(position[ii])
2. self.pose = np.array(new_positions + list(angles))
1. self.time += self.dt
if self.time > self.runtime:
self.done = True
return self.done
- 位姿：
