PX4无人机 |
您所在的位置：网站首页 › 无人机报告位置 › PX4无人机 |
PX4无人机
|
原文链接PX4无人机-Gazebo仿真实现移动物体的跟踪末尾有演示视频
这个学期我们有一个智能机器人系统的课设，我们组分配到的题目是《仿真环境下使用无人机及相机跟踪移动物体》，本文主要记录完成该课设的步骤以及内容。我们采用的最终方案是PX4飞控+gazebo仿真+mavros通讯控制，实现了在gazebo环境下无人机跟踪一个移动的小车。本文所使用的是Ubuntu18.04 + melodic。 试验环境介绍首先要搞懂各个部分的关系1，以及各自的作用，才能对控制无人机有个完整的认识，我在一开始做的时候就花了很多时间都没搞懂PX4到底是个无人机还是个什么东西，mavros又是干什么的。下面我简要介绍一下各个部分的关系，让大家有个大致的了解。 PX4飞控PX4是一个飞控固件，所谓的飞控固件，就是能够向无人机发出控制命令，控制无人机的位姿、飞行速度以及螺旋桨的转速等等。无人机的运动就需要通过飞控固件发出命令来控制。官网的用户手册在这，推荐看英文版本，中文版本有的地方翻译的实在是太烂了，我看的时候感觉像是机翻的，而且与原文的位置都不太一样。 Gazebo仿真gazebo仿真就不用多说了吧，在学ros基本的操作的时候就应该接触过gazebo。这就是一个能够模拟现实世界的仿真软件，PX4的源代码里就提供了PX4无人机的gazebo模型，通过launch文件直接运行就能得到一个gazebo下的无人机。 MAVROS通讯mavros里面有个ros，一看就是和ros相关的。我们看官网的介绍MAVROS – MAVLink extendable communication node for ROS with proxy for Ground Control Station’，这句话的意思是，MAVROS是MAVLink为了让ROS代理控制站的扩展交流节点。首先MAVLink是一个无人机通讯协议，也就是说与无人机交流所发出的信号或数据格式都要符合该协议，与HTTP等协议是一个道理。然后控制站其实是PX4为了控制无人机所开发的一个图形化控制站，可以通过GUI的形式来操作无人机，给一般用户很好的体验。而这里是用来代理控制站，也就是说充当控制站来控制无人机。到这里就很明显了，MAVROS就相当于代码版的控制站，若想要通过ros节点开控制无人机的飞行，那就必须通过mavros这个包，在这个包内包含了控制无人机的消息格式等。 综上所述，整个无人机的控制逻辑就是，通过mavros向PX4飞控发送控制命令，PX4再将命令发送到无人机的各个组件，以控制无人机按照用户的逻辑进行运动。而该无人机就在gazebo中，在gazebo中可以看到无人机的运动情况。 实验过程 PX4无人机的安装 1、安装环境依赖 sudo apt install -y ninja-build exiftool python-argparse python-empy python-toml python-numpy python-yaml python-dev python-pip ninja-build protobuf-compiler libeigen3-dev genromfs xmlstarlet libgstreamer1.0-dev libgstreamer-plugins-base1.0-dev 2、安装python依赖（melodic默认的是python2） pip install pandas jinja2 pyserial cerberus pyulog numpy toml pyquaternion -i https://pypi.tuna.tsinghua.edu.cn/simple我安装的时候pyulog没装上，其他的都装上了，如果你们也有这个问题，就把其他的装上，pyulog后面还有个步骤会自动安装 3、安装ros与gazebo这两个的安装就不多说了，如果没安装的话可以在网上先安装好这两个再继续操作。 4、安装mavros sudo apt install ros-melodic-mavros ros-melodic-mavros-extras wget https://gitee.com/robin_shaun/XTDrone/raw/master/sitl_config/mavros/install_geographiclib_datasets.sh sudo chmod a+x ./install_geographiclib_datasets.sh sudo ./install_geographiclib_datasets.sh #这步需要装一段时间,请耐心等待PX4配置我在安装的时候出现两个问题。 第一个问题是sudo apt install包的时候一直出现404，未找到这个包，解决方案时sudo apt update，将软件源更新一下，很可能是原始的位置已经过期了，需要更新才能找到最新的位置。 第二个问题是最后一步的.sh文件执行太慢了，我挂那两三个小时都没结束，大概是因为被墙了吧。解决办法如下2： 在 /usr/share 下目录新建 GeographicLib 目录。 将 geoids gravity magnetic 三个文件夹拷贝到 /usr/share/GeographicLib 文件夹下面。 上述三个文件夹的链接在这，提取码：dje9 5、PX4安装这里推荐使用gitee安装，非常感谢XTDrone团队3将代码放在了gitee上，我也使用过github安装，但总是断开连接，无数次重试才完整安装完成。 cd ~ git clone https://gitee.com/robin_shaun/PX4_Firmware cd PX4_Firmware git checkout -b xtdrone/dev v1.11.0-beta1 bash ./Tools/setup/ubuntu.sh --no-nuttx --no-sim-tools将.gitmodules替换为如下内容（在PX4_Firmware文件夹中ctrl+h查看隐藏文件）4 [submodule "mavlink/include/mavlink/v2.0"] path = mavlink/include/mavlink/v2.0 url = https://gitee.com/robin_shaun/c_library_v2.git branch = master [submodule "src/drivers/uavcan/libuavcan"] path = src/drivers/uavcan/libuavcan url = https://gitee.com/robin_shaun/uavcan.git branch = px4 [submodule "Tools/jMAVSim"] path = Tools/jMAVSim url = https://gitee.com/robin_shaun/jMAVSim.git branch = master [submodule "Tools/sitl_gazebo"] path = Tools/sitl_gazebo url = https://gitee.com/robin_shaun/sitl_gazebo.git branch = master [submodule "src/lib/matrix"] path = src/lib/matrix url = https://gitee.com/robin_shaun/Matrix.git branch = master [submodule "src/lib/ecl"] path = src/lib/ecl url = https://gitee.com/robin_shaun/ecl.git branch = master [submodule "boards/atlflight/cmake_hexagon"] path = boards/atlflight/cmake_hexagon url = https://gitee.com/robin_shaun/cmake_hexagon.git branch = px4 [submodule "src/drivers/gps/devices"] path = src/drivers/gps/devices url = https://gitee.com/robin_shaun/GpsDrivers.git branch = master [submodule "src/modules/micrortps_bridge/micro-CDR"] path = src/modules/micrortps_bridge/micro-CDR url = https://gitee.com/robin_shaun/micro-CDR.git branch = px4 [submodule "platforms/nuttx/NuttX/nuttx"] path = platforms/nuttx/NuttX/nuttx url = https://gitee.com/robin_shaun/NuttX.git branch = px4_firmware_nuttx-9.1.0+ [submodule "platforms/nuttx/NuttX/apps"] path = platforms/nuttx/NuttX/apps url = https://gitee.com/robin_shaun/NuttX-apps.git branch = px4_firmware_nuttx-9.1.0+ [submodule "platforms/qurt/dspal"] path = platforms/qurt/dspal url = https://gitee.com/robin_shaun/dspal.git [submodule "Tools/flightgear_bridge"] path = Tools/flightgear_bridge url = https://gitee.com/robin_shaun/PX4-FlightGear-Bridge.git branch = master [submodule "Tools/jsbsim_bridge"] path = Tools/jsbsim_bridge url = https://gitee.com/robin_shaun/px4-jsbsim-bridge.git [submodule "src/examples/gyro_fft/CMSIS_5"] path = src/examples/gyro_fft/CMSIS_5 url = https://gitee.com/mirrors/CMSIS_5再次执行子模块更新指令 git submodule update --init --recursive编译 make px4_sitl_default gazebo配置环境变量，注意路径的匹配，你若修改了文件夹名要进行对应的修改，第一个catkin_ws是自己的工作目录。 source ~/catkin_ws/devel/setup.bash source ~/PX4_Firmware/Tools/setup_gazebo.bash ~/PX4_Firmware/ ~/PX4_Firmware/build/px4_sitl_default export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:~/PX4_Firmware export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:~/PX4_Firmware/Tools/sitl_gazebo下面我们就可以测试PX4无人机了，执行下面的命令 cd ~/PX4_Firmware roslaunch px4 mavros_posix_sitl.launch此时会打开gazebo环境，里面地面上有一个无人机。 若显示的消息中出现connected: True,则说明MAVROS与SITL通信成功。到此，无人机的配置就结束了。 移动小车的安装由于实验要求的是实现移动物体的跟踪，因此我使用了一个可控制的小车来代替移动物体5，通过键盘控制节点可以控制小车在gazebo环境中移动。 本试验使用的是TurtleBot3小车，安装和控制移动都非常方便。 安装小车命令6 sudo apt-get install ros-melodic-turtlebot3-*通过上述命令就安装好了TurtleBot小车，是不是很方便。 由于该小车有三种形态，所以还需要通过环境变量指定一种形态，否则无法运行，我实验中使用的是 burger形态，其他两种形态你们可以自己去修改，我下面都以burger形态小车来讲解。通过环境变量指定小车有一下两种方式，推荐第二种一劳永逸，但若要修改就需要进入.bashrc文件中修改 export TURTLEBOT3_MODEL=burger # 每次打开新的终端都要执行 echo "export TURTLEBOT3_MODEL=burger" >> ~/.bashrc #直接写入环境变量，打开终端每次都会自动执行下面我们运行小车，测试一下小车控制 roslaunch turtlebot3_gazebo turtlebot3_world.launch然后运行键盘控制节点 rosrun teleop_twist_keyboard teleop_twist_keyboard.py若没安装该节点需要先安装，执行下面的命令 sudo apt-get install ros-melodic-teleop-twist-keyboard
通过上面的工作，我们完成了无人机和移动物体的配置，若要完成无人机通过相机追踪小车，那相机怎么能少得了呢？默认的PX4无人机是不带摄像头的，我们需要修改配置文件使其带上一个摄像头。 给PX4添加一个深度摄像机7 cd ~/PX4_Firmware/launch cp mavros_posix_sitl.launch mavros_posix_sitl_cp.launch # 不修改原始无人机文件，复制一个副本进行修改 gedit mavros_posix_sitl_cp.launch我后面的修改操作都是基于副本，先复制一份然后操作副本，以保持源代码的结构 做如下改动 添加 修改 为 注意添加的代码需要在修改的上方添加摄像头就完成了，但是该摄像头默认的分辨率是48 * 64，非常低，飞高一些就看不清地面的小车了，我们还需要修改一下摄像头的分辨率。 cd ~/PX4_Firmware/Tools/sitl_gazebo/models/depth_camera gedit depth_camera.sdf将对应部分修改为 10 ... R8G8B8 400 400width与height对应的就是摄像头的分辨率，update_rate是图像的发布频率，由于把像素改高了，怕系统处理速度慢，因此把频率降低一倍。 下面我们来测试一下摄像头是否配置成功。 cd ~/PX4_Firmware roslaunch px4 mavros_posix_sitl_cp.launch # 注意我修改的都是副本，不要运行错了
先添加一个接收图像的窗口
PX4仿真环境的配置文件是/home/ljw/PX4_Firmware/launch/posix_sitl.launch，与之前一样，我们不对原始文件进行修改，我们修改副本。 cd ~/PX4_Firmware/launch cp posix_sitl.launch posix_sitl_cp.launch gedit posix_sitl_cp.launch将 修改为 也就是将原本的gazebo的.world文件换成turtlebot3小车的empty.world文件，这个world里什么都没有。光这样修改还没有生效，因为我们修改的是副本，原始调用这个文件的文件也需要修改，调用这个文件的文件就是之前的mavros_posix_sitl_cp.launch gedit mavros_posix_sitl_cp.launch将 修改为 到此，无人机的配置就已经完成，可以再次运行一次无人机，看看环境是否发生变化。 合并无人机和移动小车在前面的步骤中，我们将无人机和移动小车都准备好了，下面我们就只要将无人机和小车放在同一环境中，就能开始我们的跟踪实验了。 通过阅读turtlebot3的启动文件/opt/ros/melodic/share/turtlebot3_gazebo/turtlebot3_empty_world.launch，可以看到启动小车的代码，我们将该代码添加到启动无人机的文件中，就能同时启动小车和无人机。 cd ~/PX4_Firmware/launch gedit posix_sitl_cp.launch添加如下代码 我们让小车出生在1 1位置，无人机默认出生在0 0 位置。为了让小车更容易被无人机识别，我们将小车全身改成黑色。 roscd turtlebot3_description cd urdf sudo gedit turtlebot3_burger.gazebo.xacro注意选择自己的小车文件进行修改，我这里修改的是burger小车的。 将文件中所有Gazebo/xxx中的xxx全部改成Black。 到此，无人机与移动小车全部配置完毕执行，我们测试一下。 cd ~/PX4_Firmware roslaunch px4 mavros_posix_sitl_cp.launch
通过阅读PX4官网的一个控制实例8，我们可以大致了解无人机运动的控制流程。我将实例代码复制过来并且添加注释，让大家对无人机控制代码有个理解。 /* 头文件，包括常见的geometry_msgs和mavros通信需要的mavros_msgs，添加上就行 */ #include #include #include #include #include /* current_state表示的是无人机的状态，在主函数中订阅了对应的话题，这个状态就会不断更新，表示无人机当前的状态。state_cb就是对应的回调函数，会不断的执行，更新状态。现在先不用管为什么需要这个状态，后面的代码会解释。 */ mavros_msgs::State current_state; void state_cb(const mavros_msgs::State::ConstPtr& msg){ current_state = *msg; } int main(int argc, char **argv) { ros::init(argc, argv, "offb_node"); ros::NodeHandle nh; // 订阅无人机的状态 ros::Subscriber state_sub = nh.subscribe ("mavros/state", 10, state_cb); /* 发布一个geometry_msgs::PoseStamped的消息，需要知道的是，这个消息是控制无人机的一种方式，将指定坐标包裹进这个消息，然后发布出去，无人机就能自动飞行到指定的坐标地点 */ ros::Publisher local_pos_pub = nh.advertise ("mavros/setpoint_position/local", 10); /* 无人机有一个锁，如果不解锁，无人机虽然接受了命令但是不会动被锁住了，只有解锁了才能对无人机进行控制，下面这个服务调用就是用来请求解锁无人机。上面的current_state就包含了无人机是否解锁的信息，若没解锁就需要解锁，否则就不用，其用途在这就体现出来 */ ros::ServiceClient arming_client = nh.serviceClient ("mavros/cmd/arming"); /* 无人机飞行有很多种模式，如果需要用代码操控无人机，我们就需要切换到OFFBOARD模式。上面的current_state也包含了无人机当前的飞行模式，若不是OFFBOARD就需要切换到该模式。下面的这个服务调用就是用来请求切换无人机飞行模式。 */ ros::ServiceClient set_mode_client = nh.serviceClient ("mavros/set_mode"); //the setpoint publishing rate MUST be faster than 2Hz /* 在OFFBOARD模式下，需要以>=2HZ的频率向无人机发送消息，否则无人机会回退到OFFBOARD模式之前所在的模式，因此这里的rate需要设置的比2大就行 */ ros::Rate rate(20.0); // wait for FCU connection /* 等待无人机与控制站连接（代码的方式就是代理），只有连接了才能发送消息 */ while(ros::ok() && !current_state.connected){ ros::spinOnce(); rate.sleep(); } /* pose就是坐标，本实例是让无人机在2m处悬空，因此z设置为2，z表示的就是高度 */ geometry_msgs::PoseStamped pose; pose.pose.position.x = 0; pose.pose.position.y = 0; pose.pose.position.z = 2; // 下面这个感觉有没有都无所谓 //send a few setpoints before starting for(int i = 100; ros::ok() && i > 0; --i){ local_pos_pub.publish(pose); ros::spinOnce(); rate.sleep(); } // 请求的切换模式的消息，设置为OFFBOARD mavros_msgs::SetMode offb_set_mode; offb_set_mode.request.custom_mode = "OFFBOARD"; // 请求解锁的消息，arm表示解锁，设置为true，disarm是上锁 mavros_msgs::CommandBool arm_cmd; arm_cmd.request.value = true; // 记录上次请求的时间 ros::Time last_request = ros::Time::now(); while(ros::ok()){ // 如果无人机模式不是OFFBOARD并且离上次操作时间大于5秒就发送请求切换，这里的5s是为了演示清楚设置的延时 if( current_state.mode != "OFFBOARD" && (ros::Time::now() - last_request > ros::Duration(5.0))){ if( set_mode_client.call(offb_set_mode) && offb_set_mode.response.mode_sent){ ROS_INFO("Offboard enabled"); } // 更新本次请求的时间 last_request = ros::Time::now(); } else { // 如果当前未解锁且与请求时间大于5s，就发送请求解锁 if( !current_state.armed && (ros::Time::now() - last_request > ros::Duration(5.0))){ if( arming_client.call(arm_cmd) && arm_cmd.response.success){ ROS_INFO("Vehicle armed"); } last_request = ros::Time::now(); } } // 不断发送位置消息，但是只有解锁后才能真正开始运动，如果不发送就会退出OFFBOARD模式，因为请求发送速度要>=2HZ local_pos_pub.publish(pose); ros::spinOnce(); rate.sleep(); } return 0; }通过官网的代码就能知道控制无人机飞行的一般流程，只要将核心代码逻辑修改一下，就能实现无人机对小车的跟踪了。可以先将上面的代码运行一次，观察一下无人机的运动。 总体流程为： 1、通过launch文件启动无人机的gazebo环境 2、新建一个工作空间，包含的依赖有roscpp、std_msgs、geometry_msgs、mavros、cv_bridge、image_transport、sensor_msgs 其中后面几个是为了后续图像处理用的，这里一并导入 3、创建一个cpp文件，将上述代码复制进去 4、修改CMakeLists 5、rosrun该节点 在无人机解锁后就能看到无人机飞到了空中2m处 控制无人机跟踪运动小车通过上述无人机悬空的代码我们已经了解了控制无人机飞行的代码流程：首先定义好需要发送与接收的话题消息，并且定义好各个请求，然后将无人机切换到OFFBOARD模式，接着解锁无人机，同时需要一直给无人机发送运动控制的消息，包括位置控制或速度控制，并且频率要大于2HZ。通过以上流程框架，我们就能设计一个自动跟踪移动小车的代码。 通过网上查阅资料看到，控制无人机运动不仅可以通过发送位置消息9，还可以像控制小乌龟一样，发送速度消息10，这就为跟踪小车的提供了方案。我设计的跟踪思路： 之前案例订阅和发布的话题就不用多说了，全部都要订阅，然后需要额外订阅的是无人机发送的图像，之前设置了10HZ，也就是一秒钟无人机会发送十帧图像过来，在该订阅的回调函数内，对图像进行处理，检查图像内是否有小车，如果有的话，就通过比较小车像素点的位置和图像中心像素点的位置，来判断方位，并相应的设置速度。图像处理回调函数如下： #include #include #include #include #include #include #include #include "sensor_msgs/Image.h" #include "cv_bridge/cv_bridge.h" #include #include #include // 一些全局变量 // 悬空高度（追踪小车的高度） const double h = 4; // 调整高度的速度（上升或下降） const double hv = 0.1; // 控制无人机的速度 geometry_msgs::Twist velocity; // 无人机当前的高度 double curH; // 无人机是否已经稳定在空中的标志 bool start = false; void doImg(const sensor_msgs::Image::ConstPtr &msg) { if(!start) return; // 将无人机发布的图像先转化为灰度图，再进行二值化，就能得到黑白图像，若小车出现，那么在图像内有黑色的像素，否则图像全是白色像素，这也是我将小车改成黑色的原因，若改成其它颜色就不好进行分离 cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8); cv::Mat img = cv_ptr -> image; cv::Mat gray, bin; cv::cvtColor(img, gray, cv::COLOR_BGR2GRAY); cv::threshold(gray, bin, 127, 255, cv::THRESH_BINARY); // 获得图像的宽和高 static int row = bin.rows, col = bin.cols; // 图像中心点的位置，我们假设图像中心点的位置就是无人机的位置，这样就能很方便的发布速度来控制无人机 static double centX = row / 2, centY = col / 2; // x y用来记录小车在该帧图像出现的位置 int x, y; // 是否找到小车的标记 bool findCar = false; // 遍历图像，若图像内有黑色像素则代表发现了小车，记录下此时的x y位置 for(int i = 0; i |
最后一步，测试无人机通讯
通过I L J K ,四个键可以控制小车的运动，详细的运动控制自己看控制台的输出。到此，移动小车的安装就已经完成。
然后打开rviz，新开一个终端输入
将图像的话题选择为/camera/rgb/image_raw，另一个对应的是深度图像，可以自己切换看看效果。
这个时候image窗口就会显示无人机摄像机拍摄下来的图像，然后在运行无人机的那个终端输入命令commander takeoff，观察该图像窗口，会随着无人机起飞变化。
由于这个地面平坦，将小车放到这个环境中的话，小车会动不了，所以要将仿真环境改一下，改成原始的空仿真环境。
我们可以看到中间一个无人机和一个黑乎乎的小车，周围的环境已经变成了空的了。然后启动键盘控制节点可以控制小车的运动【本文地址】