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

2023-12-24 17:45
文章标签 实现 定点 无人机 大疆 飞行 webots mavic2pro

本文主要是介绍Webots实现大疆Mavic2pro无人机定点飞行，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！

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

文章目录

  • 前言
  • 一、将无人机当成一个对象
    • 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控制器实现键盘控制无人机运动

四、注意点

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

这篇关于Webots实现大疆Mavic2pro无人机定点飞行的文章就介绍到这儿，希望我们推荐的文章对编程师们有所帮助！


http://www.chinasem.cn/article/532600。

相关文章

hdu1043（八数码问题，广搜 + hash（实现状态压缩） ）

hdu1043（八数码问题，广搜 + hash（实现状态压缩） ）

利用康拓展开将一个排列映射成一个自然数，然后就变成了普通的广搜题。 #include<iostream>#include<algorithm>#include<string>#include<stack>#include<queue>#include<map>#include<stdio.h>#include<stdlib.h>#include<ctype.h>#inclu
阅读更多...
【专题】2024飞行汽车技术全景报告合集PDF分享（附原数据表）

【专题】2024飞行汽车技术全景报告合集PDF分享（附原数据表）

原文链接： https://tecdat.cn/?p=37628 6月16日，小鹏汇天旅航者X2在北京大兴国际机场临空经济区完成首飞，这也是小鹏汇天的产品在京津冀地区进行的首次飞行。小鹏汇天方面还表示，公司准备量产，并计划今年四季度开启预售小鹏汇天分体式飞行汽车，探索分体式飞行汽车城际通勤。阅读原文，获取专题报告合集全文，解锁文末271份飞行汽车相关行业研究报告。 据悉，业内人士对飞行汽车行业
阅读更多...
【C++】_list常用方法解析及模拟实现

【C++】_list常用方法解析及模拟实现

相信自己的力量，只要对自己始终保持信心，尽自己最大努力去完成任何事，就算事情最终结果是失败了，努力了也不留遗憾。💓💓💓 目录   ✨说在前面 🍋知识点一：什么是list？ •🌰1.list的定义 •🌰2.list的基本特性 •🌰3.常用接口介绍 🍋知识点二：list常用接口 •🌰1.默认成员函数 🔥构造函数（⭐） 🔥析构函数 •🌰2.list对象
阅读更多...
【Prometheus】PromQL向量匹配实现不同标签的向量数据进行运算

【Prometheus】PromQL向量匹配实现不同标签的向量数据进行运算

✨✨ 欢迎大家来到景天科技苑✨✨ 🎈🎈 养成好习惯，先赞后看哦~🎈🎈 🏆 作者简介：景天科技苑 🏆《头衔》：大厂架构师，华为云开发者社区专家博主，阿里云开发者社区专家博主，CSDN全栈领域优质创作者，掘金优秀博主，51CTO博客专家等。 🏆《博客》：Python全栈，前后端开发，小程序开发，人工智能，js逆向，App逆向，网络系统安全，数据分析，Django，fastapi
阅读更多...
让树莓派智能语音助手实现定时提醒功能

让树莓派智能语音助手实现定时提醒功能

最初的时候是想直接在rasa 的chatbot上实现，因为rasa本身是带有remindschedule模块的。不过经过一番折腾后，忽然发现，chatbot上实现的定时，语音助手不一定会有响应。因为，我目前语音助手的代码设置了长时间无应答会结束对话，这样一来，chatbot定时提醒的触发就不会被语音助手获悉。那怎么让语音助手也具有定时提醒功能呢？ 我最后选择的方法是用threading.Time
阅读更多...
Android实现任意版本设置默认的锁屏壁纸和桌面壁纸（两张壁纸可不一致）

Android实现任意版本设置默认的锁屏壁纸和桌面壁纸（两张壁纸可不一致）

客户有些需求需要设置默认壁纸和锁屏壁纸  在默认情况下 这两个壁纸是相同的  如果需要默认的锁屏壁纸和桌面壁纸不一样 需要额外修改 Android13实现 替换默认桌面壁纸： 将图片文件替换frameworks/base/core/res/res/drawable-nodpi/default_wallpaper.*  （注意不能是bmp格式） 替换默认锁屏壁纸： 将图片资源放入vendo
阅读更多...
C#实战|大乐透选号器[6]：实现实时显示已选择的红蓝球数量

C#实战|大乐透选号器[6]：实现实时显示已选择的红蓝球数量

哈喽，你好啊，我是雷工。 关于大乐透选号器在前面已经记录了5篇笔记，这是第6篇； 接下来实现实时显示当前选中红球数量，蓝球数量； 以下为练习笔记。 01 效果演示 当选择和取消选择红球或蓝球时，在对应的位置显示实时已选择的红球、蓝球的数量； 02 标签名称 分别设置Label标签名称为：lblRedCount、lblBlueCount
阅读更多...
Kubernetes PodSecurityPolicy：PSP能实现的5种主要安全策略

Kubernetes PodSecurityPolicy：PSP能实现的5种主要安全策略

Kubernetes PodSecurityPolicy：PSP能实现的5种主要安全策略 1. 特权模式限制2. 宿主机资源隔离3. 用户和组管理4. 权限提升控制5. SELinux配置 💖The Begin💖点点关注，收藏不迷路💖 Kubernetes的PodSecurityPolicy（PSP）是一个关键的安全特性，它在Pod创建之前实施安全策略，确保P
阅读更多...
工厂ERP管理系统实现源码（JAVA）

工厂ERP管理系统实现源码（JAVA）

工厂进销存管理系统是一个集采购管理、仓库管理、生产管理和销售管理于一体的综合解决方案。该系统旨在帮助企业优化流程、提高效率、降低成本，并实时掌握各环节的运营状况。 在采购管理方面，系统能够处理采购订单、供应商管理和采购入库等流程，确保采购过程的透明和高效。仓库管理方面，实现库存的精准管理，包括入库、出库、盘点等操作，确保库存数据的准确性和实时性。 生产管理模块则涵盖了生产计划制定、物料需求计划、
阅读更多...
C++——stack、queue的实现及deque的介绍

C++——stack、queue的实现及deque的介绍

目录 1.stack与queue的实现 1.1stack的实现  1.2 queue的实现 2.重温vector、list、stack、queue的介绍 2.1 STL标准库中stack和queue的底层结构  3.deque的简单介绍 3.1为什么选择deque作为stack和queue的底层默认容器  3.2 STL中对stack与queue的模拟实现 ①stack模拟实现
阅读更多...

Copyright China编程 23002807@qq.com

沪ICP备2023033072号-2