【Tello无人机】Tello飞行控制

【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_info

step函数，与环境交互接口

点击【Tello无人机】Tello飞行控制 - 古月居可查看全文