Webots实现大疆Mavic2pro无人机定点飞行

提示：文章写完后，目录可以自动生成，如何生成可参考右边的帮助文档

---

前言

由于项目要求，现在需要做一个能够实现无人机根据事先给定的点位实现定点飞行，这里由于webots的跨平台性，考虑使用webots进行仿真

一、将无人机当成一个对象

1.1定义无人机相关属性

由于无人机有pitch、yaw、roll三个属性，分别对应前后运动、左右偏航和左右横滚、这里定义相关的所有属性用于控制。
同时定义相应的用于控制运动的函数

1.2定义用于控制无人机运动的代码

import math
import time
from controller import Robot, Camera, Compass, GPS, Gyro, InertialUnit, Keyboard, LED, Motor

# 自定义无人机类，继承机器人父类
class UAV(Robot):

    timestep = 0
    # Constants, empirically found.
    k_vertical_thrust = 68.5  # with this thrust, the drone lifts.
    k_vertical_offset = 0.6   # Vertical offset where the robot actually targets to stabilize itself.
    k_vertical_p = 3.0        # P constant of the vertical PID.
    k_roll_p = 50.0           # P constant of the roll PID.
    k_pitch_p = 30.0          # P constant of the pitch PID.
    # 初始化变量
    def __init__(self):
        # Get and enable devices.
        self.camera = Camera("camera")
        self.camera.enable(timestep)
        self.front_left_led = LED("front left led")
        self.front_right_led = LED("front right led")
        self.imu = InertialUnit("inertial unit")
        self.imu.enable(timestep)
        self.gps = GPS("gps")
        self.gps.enable(timestep)
        self.compass = Compass("compass")
        self.compass.enable(timestep)
        # 检测角速度
        self.gyro = Gyro("gyro")
        self.gyro.enable(timestep)
        # keyboard = Keyboard()
        # keyboard.enable(timestep)
        # 横滚检测器
        self.camera_roll_motor = Motor("camera roll")
        # 前后俯仰检测器
        self.camera_pitch_motor = Motor("camera pitch")
        # 用于控制无人机平稳飞行的变量
        self.roll_disturbance = 0.0
        self.pitch_disturbance = 0.0
        self.yaw_disturbance = 0.0
        # 设置初始目标噶度
        self.target_altitude = 10.0
        # Get propeller motors and set them to velocity mode.
        self.front_left_motor = Motor("front left propeller")
        self.front_right_motor = Motor("front right propeller")
        self.rear_left_motor = Motor("rear left propeller")
        self.rear_right_motor = Motor("rear right propeller")
        # 将所有的驱动器保存到一个数组中
        self.motors = [self.front_left_motor, self.front_right_motor, self.rear_left_motor, self.rear_right_motor]
    
    # 前进
    def forward():
        self.pitch_disturbance = 2.0
    # 后退
    def backward():
        self.pitch_disturbance = -2.0
    # 向右运动
    def right():
        self.yaw_disturbance = 1.3
    # 向左运动
    def left():
        self.yaw_disturbance = -1.3
    # 向右横滚
    def roll_right():
        self.roll_disturbance = -1.0
    # 向左横滚
    def roll_left():
        self.roll_disturbance = 1.0
    # 上升
    def up():
        self.target_altitude += 0.05
        print("target altitude:", target_altitude, "[m]")
    # 下降
    def down():
        self.target_altitude -= 0.05
        print("target altitude:", target_altitude, "[m]")
    # 获取无人机当前位置
    def getPosition():
        self.roll = self.imu.getRollPitchYaw()[0] + math.pi / 2.0
        self.pitch = self.imu.getRollPitchYaw()[1]
        self.altitude = self.gps.getValues()[1]
        # 获取角速度
        self.roll_acceleration = self.gyro.getValues()[0]
        self.pitch_acceleration = self.gyro.getValues()[1]

        # Blink the front LEDs alternatively with a 1 second rate.
        self.led_state = int(time) % 2
        self.front_left_led.set(led_state)
        self.front_right_led.set(1 - led_state)

    # 根据相关参数进行运动控制
    def Move():
        # Stabilize the Camera by actuating the camera motors according to the gyro feedback.
        self.camera_roll_motor.setPosition(-0.115 * self.roll_acceleration)
        self.camera_pitch_motor.setPosition(-0.1 * self.pitch_acceleration)

        # Compute the roll, pitch, and yaw errors.
        roll_input = self.k_roll_p * CLAMP(self.roll, -1.0, 1.0) + self.roll_acceleration + self.roll_disturbance
        pitch_input = self.k_pitch_p * CLAMP(self.pitch, -1.0, 1.0) - self.pitch_acceleration + self.pitch_disturbance
        yaw_input = self.yaw_disturbance
        clamped_difference_altitude = CLAMP(self.target_altitude - self.altitude + self.k_vertical_offset, -1.0, 1.0)
        vertical_input = self.k_vertical_p * pow(clamped_difference_altitude, 3.0)


        # Accute the motor taking into consideration all the computed inputs.
        front_left_motor_input = self.k_vertical_thrust + vertical_input - roll_input - pitch_input + yaw_input
        front_right_motor_input = self.k_vertical_thrust + vertical_input + roll_input - pitch_input - yaw_input
        rear_left_motor_input = self.k_vertical_thrust + vertical_input - roll_input + pitch_input - yaw_input
        rear_right_motor_input = self.k_vertical_thrust + vertical_input + roll_input + pitch_input + yaw_input
        self.front_left_motor.setVelocity(front_left_motor_input)
        self.front_right_motor.setVelocity(-front_right_motor_input)
        self.rear_left_motor.setVelocity(-rear_left_motor_input)
        self.rear_right_motor.setVelocity(rear_right_motor_input)

# 辅助函数
def CLAMP(value, low, high):
    return max(low, min(value, high))

1.3主函数实现无人机的点位固定和飞行检测

将主函数声明成控制器就可以了

from Uav import Uav
def main():
    uav = Uav()
    timestep = int(uav.getBasicTimeStep())
    uav.timestep = timestep
    keyboard = Keyboard()
    keyboard.enable(timestep)
    while uav.step(timestep) != -1:
        key = keyboard.getKey()
        uav.roll_disturbance = 0.0
        uav.pitch_disturbance = 0.0
        uav.yaw_disturbance = 0.0
        while key > 0:
            # 上升函数
            if key == Keyboard.UP:
                uav.forward()
            elif key == Keyboard.DOWN:
                uav.backward()
            elif key == Keyboard.RIGHT:
                uav.right()
            elif key == Keyboard.LEFT:
                uav.left()
            elif key == (Keyboard.SHIFT + Keyboard.RIGHT):
                uav.roll_right()
            elif key == (Keyboard.SHIFT + Keyboard.LEFT):
                uav.roll_left()
            elif key == (Keyboard.SHIFT + Keyboard.UP):
                uav.up()
            elif key == (Keyboard.SHIFT + Keyboard.DOWN):
                uav.down()
            key = keyboard.getKey()
        uav.getPosition()
        uav.Move()
    wb_robot_cleanup();

if __name__ == "__main__" :
    main()

二、用键盘控制测试代码

由于webots默认给的是通过C++代码实现键盘对无人机进行控制，然而开发使用的多是python，这里给出根据原本C++代码改写的python控制代码，直接新建成一个控制器然后在webots中选择这个.py文件作为控制器就可以了，记得放到controler文件夹中。

import math
import time
from controller import Robot, Camera, Compass, GPS, Gyro, InertialUnit, Keyboard, LED, Motor

def CLAMP(value, low, high):
    return max(low, min(value, high))

def main():
    # 创建一个机器人对象
    robot = Robot()
    # 每个物理动作的持续时间
    timestep = int(robot.getBasicTimeStep())

    # Get and enable devices.
    camera = Camera("camera")
    camera.enable(timestep)
    front_left_led = LED("front left led")
    front_right_led = LED("front right led")
    imu = InertialUnit("inertial unit")
    imu.enable(timestep)
    gps = GPS("gps")
    gps.enable(timestep)
    compass = Compass("compass")
    compass.enable(timestep)
    # 检测角速度
    gyro = Gyro("gyro")
    gyro.enable(timestep)
    keyboard = Keyboard()
    keyboard.enable(timestep)
    # 横滚检测器
    camera_roll_motor = Motor("camera roll")
    # 前后俯仰检测器
    camera_pitch_motor = Motor("camera pitch")

    # Get propeller motors and set them to velocity mode.
    front_left_motor = Motor("front left propeller")
    front_right_motor = Motor("front right propeller")
    rear_left_motor = Motor("rear left propeller")
    rear_right_motor = Motor("rear right propeller")
    motors = [front_left_motor, front_right_motor, rear_left_motor, rear_right_motor]
    for motor in motors:
        # 初始化无限旋转的运动
        motor.setPosition(float('inf'))
        # 启动！
        motor.setVelocity(1.0)

    # Display the welcome message.
    print("Start the drone...")

    # Wait one second.
    while robot.step(timestep) != -1:
        if robot.getTime() > 1.0:
            break

    # Display manual control message.
    print("You can control the drone with your computer keyboard:")
    print("- 'up': move forward.")
    print("- 'down': move backward.")
    print("- 'right': turn right.")
    print("- 'left': turn left.")
    print("- 'shift + up': increase the target altitude.")
    print("- 'shift + down': decrease the target altitude.")
    print("- 'shift + right': strafe right.")
    print("- 'shift + left': strafe left.")

    # Constants, empirically found.
    k_vertical_thrust = 68.5  # with this thrust, the drone lifts.
    k_vertical_offset = 0.6   # Vertical offset where the robot actually targets to stabilize itself.
    k_vertical_p = 3.0        # P constant of the vertical PID.
    k_roll_p = 50.0           # P constant of the roll PID.
    k_pitch_p = 30.0          # P constant of the pitch PID.

    # Variables.
    # 设置初始高度
    target_altitude = 1.0  # The target altitude. Can be changed by the user.

    # Main loop
    # - perform simulation steps until Webots is stopping the controller
    while robot.step(timestep) != -1:
        time = robot.getTime()

        # Retrieve robot position using the sensors.
        roll = imu.getRollPitchYaw()[0] + math.pi / 2.0
        pitch = imu.getRollPitchYaw()[1]
        altitude = gps.getValues()[1]
        # 获取角速度
        roll_acceleration = gyro.getValues()[0]
        pitch_acceleration = gyro.getValues()[1]

        # Blink the front LEDs alternatively with a 1 second rate.
        led_state = int(time) % 2
        front_left_led.set(led_state)
        front_right_led.set(1 - led_state)

        # Stabilize the Camera by actuating the camera motors according to the gyro feedback.
        camera_roll_motor.setPosition(-0.115 * roll_acceleration)
        camera_pitch_motor.setPosition(-0.1 * pitch_acceleration)

        # Transform the keyboard input to disturbances on the stabilization algorithm.
        roll_disturbance = 0.0
        pitch_disturbance = 0.0
        yaw_disturbance = 0.0
        key = keyboard.getKey()
        while key > 0:
            # 上升函数
            if key == Keyboard.UP:
                pitch_disturbance = 2.0
            elif key == Keyboard.DOWN:
                pitch_disturbance = -2.0
            elif key == Keyboard.RIGHT:
                yaw_disturbance = 1.3
            elif key == Keyboard.LEFT:
                yaw_disturbance = -1.3
            elif key == (Keyboard.SHIFT + Keyboard.RIGHT):
                roll_disturbance = -1.0
            elif key == (Keyboard.SHIFT + Keyboard.LEFT):
                roll_disturbance = 1.0
            elif key == (Keyboard.SHIFT + Keyboard.UP):
                target_altitude += 0.05
                print("target altitude:", target_altitude, "[m]")
            elif key == (Keyboard.SHIFT + Keyboard.DOWN):
                target_altitude -= 0.05
                print("target altitude:", target_altitude, "[m]")
            key = keyboard.getKey()

        # Compute the roll, pitch, and yaw errors.
        roll_input = k_roll_p * CLAMP(roll, -1.0, 1.0) + roll_acceleration + roll_disturbance
        pitch_input = k_pitch_p * CLAMP(pitch, -1.0, 1.0) - pitch_acceleration + pitch_disturbance
        yaw_input = yaw_disturbance
        clamped_difference_altitude = CLAMP(target_altitude - altitude + k_vertical_offset, -1.0, 1.0)
        vertical_input = k_vertical_p * pow(clamped_difference_altitude, 3.0)


        # Accute the motor taking into consideration all the computed inputs.
        front_left_motor_input = k_vertical_thrust + vertical_input - roll_input - pitch_input + yaw_input
        front_right_motor_input = k_vertical_thrust + vertical_input + roll_input - pitch_input - yaw_input
        rear_left_motor_input = k_vertical_thrust + vertical_input - roll_input + pitch_input - yaw_input
        rear_right_motor_input = k_vertical_thrust + vertical_input + roll_input + pitch_input + yaw_input
        front_left_motor.setVelocity(front_left_motor_input)
        front_right_motor.setVelocity(-front_right_motor_input)
        rear_left_motor.setVelocity(-rear_left_motor_input)
        rear_right_motor.setVelocity(rear_right_motor_input)
        
    
    wb_robot_cleanup()

if __name__ == "__main__":
    main()

三、效果展示

四、注意点

1. Webots中不支持到其他库，所以理论上应该都写在一个文件夹中，如果想要写在不用的文件夹中，需要
2. 改变控制器以后记得重新保存一份世界文件。