提示：文章写完后，目录可以自动生成，如何生成可参考右边的帮助文档
由于项目要求，现在需要做一个能够实现无人机根据事先给定的点位实现定点飞行，这里由于webots的跨平台性，考虑使用webots进行仿真
由于无人机有pitch、yaw、roll三个属性，分别对应前后运动、左右偏航和左右横滚、这里定义相关的所有属性用于控制。
同时定义相应的用于控制运动的函数
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))
将主函数声明成控制器就可以了
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()
用python控制器实现键盘控制无人机运动