【Tello无人机】Tello飞行控制
上一篇介绍了Tello无人机的仿真环境搭建，本篇将介绍tello无人机在pybullet环境中的飞行控制，实现无人机的速度控制。本环境最终要实现强化学习算法下的飞行任务，故采用通用的gym接口进行环境搭建。
Gym环境接口
OpenAI Gym 包含许多有趣的环境，从具有经典控制任务的环境到可让你训练智能体玩 Atari 游戏的环境。但本文是想自己定义一个tello无人机飞行的环境，同时为了兼容强化学习算法，采用gym接口进行封装。gym 对环境镜像了抽象，无论多复杂的环境，最终暴露出来的只有gym.Env抽象的5个方法，本文在gym-pybullet-drones代码的基础上进行修改：
BaseAviary类
class BaseAviary(gym.Env): """该类是gym.Env的子类，表明它是用于强化学习环境的基本结构。以下是对每个部分的功能说明：""" def __init__(self, num_drones: int = 1, initial_xyzs=None, initial_rpys=None, pyb_freq: int = 240, ctrl_freq: int = 240, gui=False, obstacles=False, user_control_debug=False, output_folder='results' ): """用于初始化环境的实例。接受多个参数，包括无人机数量（num_drones）、初始位置和姿态信息（initial_xyzs 和 initial_rpys）、PyBullet模拟的频率（pyb_freq）、控制频率（ctrl_freq）、是否使用GUI进行可视化（gui）、是否包含障碍物（obstacles）、是否启用用户控制调试模式（user_control_debug）以及结果输出文件夹路径（output_folder）。""" def reset(self, seed: int = None, options: dict = None): """用于重置环境的状态。可以传递种子值（seed）和其他配置选项（options）。""" def step(self, action): """实现了环境的一个时间步骤。接受动作作为输入，返回包含观察、奖励、终止标志和其他信息的元组。""" def render(self, mode='human', close=False): """用于将环境可视化。接受模式（mode）和关闭标志（close）等参数。""" def close(self): """用于关闭环境，释放资源。"""初始化函数，初始化模型信息，环境信息
def __init__(self, num_drones: int = 1, initial_xyzs=None, initial_rpys=None, pyb_freq: int = 240, ctrl_freq: int = 60, gui=False, obstacles=False, user_control_debug=False, show_traj=False, output_folder='results' ): #### Constants ############################################# self.aim_h = 0.60 self.G = 9.8 self.RAD2DEG = 180 / np.pi self.DEG2RAD = np.pi / 180 self.CTRL_FREQ = ctrl_freq self.PYB_FREQ = pyb_freq if self.PYB_FREQ % self.CTRL_FREQ != 0: # 控制频率与系统仿真频率 raise ValueError('[ERROR] in BaseAviary.__init__(), pyb_freq is not divisible by env_freq.') self.PYB_STEPS_PER_CTRL = int(self.PYB_FREQ / self.CTRL_FREQ) self.CTRL_TIMESTEP = 1. / self.CTRL_FREQ self.PYB_TIMESTEP = 1. / self.PYB_FREQ #### Parameters ############################################ self.NUM_DRONES = num_drones #### Options ############################################### self.GUI = gui self.show_traj = show_traj self.OBSTACLES = obstacles self.user_control_debug = user_control_debug self.URDF = "tello.urdf" self.OUTPUT_FOLDER = output_folder #### Load the drone properties from the .urdf file ######### 模型参数 self.M, \ self.L, \ self.THRUST2WEIGHT_RATIO, \ self.J, \ self.J_INV, \ self.KF, \ self.KM, \ self.COLLISION_H, \ self.COLLISION_R, \ self.COLLISION_Z_OFFSET, \ self.MAX_SPEED_KMH, \ self.GND_EFF_COEFF, \ self.PROP_RADIUS, \ self.DRAG_COEFF, \ self.DW_COEFF_1, \ self.DW_COEFF_2, \ self.DW_COEFF_3 = self._parseURDFParameters() print( "[INFO] BaseAviary.__init__() loaded parameters from the drone's .urdf:\n[INFO] m {:f}, L {:f},\n[INFO] ixx {:f}, iyy {:f}, izz {:f},\n[INFO] kf {:f}, km {:f},\n[INFO] t2w {:f}, max_speed_kmh {:f},\n[INFO] gnd_eff_coeff {:f}, prop_radius {:f},\n[INFO] drag_xy_coeff {:f}, drag_z_coeff {:f},\n[INFO] dw_coeff_1 {:f}, dw_coeff_2 {:f}, dw_coeff_3 {:f}".format( self.M, self.L, self.J[0, 0], self.J[1, 1], self.J[2, 2], self.KF, self.KM, self.THRUST2WEIGHT_RATIO, self.MAX_SPEED_KMH, self.GND_EFF_COEFF, self.PROP_RADIUS, self.DRAG_COEFF[0], self.DRAG_COEFF[2], self.DW_COEFF_1, self.DW_COEFF_2, self.DW_COEFF_3)) #### Compute constants ##################################### self.GRAVITY = self.G * self.M # 无人机重力 self.HOVER_RPM = np.sqrt(self.GRAVITY / (4 * self.KF)) # 计算悬停转速 self.MAX_RPM = np.sqrt((self.THRUST2WEIGHT_RATIO * self.GRAVITY) / (4 * self.KF)) # 计算最大转速 print("max_a:", self.MAX_RPM) self.MAX_THRUST = (4 * self.KF * self.MAX_RPM ** 2) # 计算最大推力 self.MAX_XY_TORQUE = (2 * self.L * self.KF * self.MAX_RPM ** 2) / np.sqrt(2) # xy方向的最大推力 self.MAX_Z_TORQUE = (2 * self.KM * self.MAX_RPM ** 2) # z方向的最大推力 self.GND_EFF_H_CLIP = 0.25 * self.PROP_RADIUS * np.sqrt( (15 * self.MAX_RPM ** 2 * self.KF * self.GND_EFF_COEFF) / self.MAX_THRUST) # 计算地面效益系数 if self.GUI: #### With debug GUI ######################################## self.CLIENT = p.connect(p.GUI) # p.connect(p.GUI, options="--opengl2") for i in [p.COV_ENABLE_RGB_BUFFER_PREVIEW, p.COV_ENABLE_DEPTH_BUFFER_PREVIEW, p.COV_ENABLE_SEGMENTATION_MARK_PREVIEW, p.COV_ENABLE_GUI]: p.configureDebugVisualizer(i, 0, physicsClientId=self.CLIENT) # TODO 相机视角 p.resetDebugVisualizerCamera(cameraDistance=2, cameraYaw=-0, cameraPitch=-80, cameraTargetPosition=[0, 0, 0], physicsClientId=self.CLIENT ) ret = p.getDebugVisualizerCamera(physicsClientId=self.CLIENT) print("viewMatrix", ret[2]) print("projectionMatrix", ret[3]) else: #### Without debug GUI ##################################### self.CLIENT = p.connect(p.DIRECT) #### Set initial poses ##################################### if initial_xyzs is None: self.INIT_XYZS = np.array([[1, -1, 0.2]]) elif np.array(initial_xyzs).shape == (self.NUM_DRONES, 3): self.INIT_XYZS = initial_xyzs else: print("[ERROR] invalid initial_xyzs in BaseAviary.__init__(), try initial_xyzs.reshape(NUM_DRONES,3)") if initial_rpys is None: self.INIT_RPYS = np.zeros((self.NUM_DRONES, 3)) elif np.array(initial_rpys).shape == (self.NUM_DRONES, 3): self.INIT_RPYS = initial_rpys else: print("[ERROR] invalid initial_rpys in BaseAviary.__init__(), try initial_rpys.reshape(NUM_DRONES,3)") #### Create action and observation spaces ################## self.quaternion_orient_zero = p.getQuaternionFromEuler([0, 0, 0]) # self.env_dim = 5.0 # 5*5m的仿真环境 self.random_pos = True self.action_space = self._actionSpace() self.observation_space = self._observationSpace() self.init_env() self._task_reset() # 加载任务场景 self._updateAndStoreKinematicInformation() self.action_list = deque(maxlen=5)复位函数，将环境恢复到初始状态
def reset(self, seed: int = None, options: dict = None): # TODO : initialize random number generator with seed p.resetSimulation(physicsClientId=self.CLIENT) self._housekeeping() self._updateAndStoreKinematicInformation() self.end_point = [0, 0, 0] initial_obs = self._computeObs() initial_info = self._computeInfo() return initial_obs, initial_infostep函数，与环境交互接口
点击【Tello无人机】Tello飞行控制 - 古月居可查看全文
