龙岩做网站改版找哪家公司,兴义网络推广,app与网站用的服务器,公众号怎么开通留言提示#xff1a;文章写完后#xff0c;目录可以自动生成#xff0c;如何生成可参考右边的帮助文档 文章目录 前言一、将无人机当成一个对象1.1定义无人机相关属性1.2定义用于控制无人机运动的代码1.3主函数实现无人机的点位固定和飞行检测 二、用键盘控制测试代码三、效果展… 提示文章写完后目录可以自动生成如何生成可参考右边的帮助文档 文章目录 前言一、将无人机当成一个对象1.1定义无人机相关属性1.2定义用于控制无人机运动的代码1.3主函数实现无人机的点位固定和飞行检测 二、用键盘控制测试代码三、效果展示四、注意点 前言
由于项目要求现在需要做一个能够实现无人机根据事先给定的点位实现定点飞行这里由于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.0self.pitch_disturbance 0.0self.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.05print(target altitude:, target_altitude, [m])# 下降def down():self.target_altitude - 0.05print(target altitude:, target_altitude, [m])# 获取无人机当前位置def getPosition():self.roll self.imu.getRollPitchYaw()[0] math.pi / 2.0self.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) % 2self.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_disturbancepitch_input self.k_pitch_p * CLAMP(self.pitch, -1.0, 1.0) - self.pitch_acceleration self.pitch_disturbanceyaw_input self.yaw_disturbanceclamped_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_inputfront_right_motor_input self.k_vertical_thrust vertical_input roll_input - pitch_input - yaw_inputrear_left_motor_input self.k_vertical_thrust vertical_input - roll_input pitch_input - yaw_inputrear_right_motor_input self.k_vertical_thrust vertical_input roll_input pitch_input yaw_inputself.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 timestepkeyboard Keyboard()keyboard.enable(timestep)while uav.step(timestep) ! -1:key keyboard.getKey()uav.roll_disturbance 0.0uav.pitch_disturbance 0.0uav.yaw_disturbance 0.0while 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, Motordef 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 controllerwhile robot.step(timestep) ! -1:time robot.getTime()# Retrieve robot position using the sensors.roll imu.getRollPitchYaw()[0] math.pi / 2.0pitch 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) % 2front_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.0pitch_disturbance 0.0yaw_disturbance 0.0key keyboard.getKey()while key 0:# 上升函数if key Keyboard.UP:pitch_disturbance 2.0elif key Keyboard.DOWN:pitch_disturbance -2.0elif key Keyboard.RIGHT:yaw_disturbance 1.3elif key Keyboard.LEFT:yaw_disturbance -1.3elif key (Keyboard.SHIFT Keyboard.RIGHT):roll_disturbance -1.0elif key (Keyboard.SHIFT Keyboard.LEFT):roll_disturbance 1.0elif key (Keyboard.SHIFT Keyboard.UP):target_altitude 0.05print(target altitude:, target_altitude, [m])elif key (Keyboard.SHIFT Keyboard.DOWN):target_altitude - 0.05print(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_disturbancepitch_input k_pitch_p * CLAMP(pitch, -1.0, 1.0) - pitch_acceleration pitch_disturbanceyaw_input yaw_disturbanceclamped_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_inputfront_right_motor_input k_vertical_thrust vertical_input roll_input - pitch_input - yaw_inputrear_left_motor_input k_vertical_thrust vertical_input - roll_input pitch_input - yaw_inputrear_right_motor_input k_vertical_thrust vertical_input roll_input pitch_input yaw_inputfront_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控制器实现键盘控制无人机运动 四、注意点
Webots中不支持到其他库所以理论上应该都写在一个文件夹中如果想要写在不用的文件夹中需要改变控制器以后记得重新保存一份世界文件。
