最好的网站设计公,html5美食网站,阜宁网页设计,贵阳网络推广公司哪家强文章主要讲述px4 如何利用vins_fusion里程计数据实现在室内定位功能。
文章基于以下软、硬件展开。
硬件软件机载电脑#xff1a; Intel NUC系统#xff1a;Ubuntu 20.04相机#xff1a; Intel Realsense D435iros#xff1a;noetic飞控#xff1a;Pixhawk 2.4.8固件 Intel NUC系统Ubuntu 20.04相机 Intel Realsense D435irosnoetic飞控Pixhawk 2.4.8固件PX4 1.14.0
完整vins_to_mavros 功能包地址 https://github.com/rotorcraftman/px4ctrl
随着slam开源技术的普及px4 要实现室内定位实现方式很多如文章使用的vins-fusion等视觉里程计激光里程计等。 本质上要实现无人机的室内定位有两个方法 1.提供室内位置信息 室外可以用gps提供位置信息实现定位而室内因为gps没有信号所以朴素的想法是只要提供位置信息给飞控就可实现像室外gps定位的效果了。 思路 vio提供里程计—(/mavros/vision_posion/pose)mavros(mavlink)—px4 可以看出mavros起到了连接vio和px4的桥梁作用。 2.offboard 外部计算机遵守照MAVLink 协议提供的位置速度或姿态设定值。 设定值可以由机载计算机上运行的 MAVLink API例如 MAVSDK (opens new window) 或 MAVROS (opens new window)提供通常通过串口或 wifi 连接。 3.光流、UWB等。
接下来详细讲述第一种实现方式vio以vins_fusion为例。
文章叙述展开方式默认已完成了px4飞控的建立、mavros的安装未完成的同志可参考上一篇文章 gazebo11px4联合仿真测试
一、Pixhawk MAVLink Ports配置
配置Pixhawk Telem2作为与机载电脑数据交互的MAVLINK端口
MAV_1_CONFIG TELEM 2 MAV_1_MODE Onboard SER_TEL2_BAUD 921600 8N1
设置前参数里可能只有MAV_1_CONFIG搜不到其他的参数将MAV_1_CONFIG设置为TELEM 2然后把飞控重启后其他参数就有了。 Pixhawk 2.4.8硬件设置为102参数对应关系如下。 详细参数介绍参见https://docs.px4.io/main/en/advanced_config/parameter_reference.html
Pixhawk 2.4.8 TELEM1/TELEM2端口设置如下
TELEM1TELEM2MAV_0_CONFIG TELEM 1MAV_1_CONFIG TELEM 2MAV_0_MODE NormalMAV_1_MODE OnboardMAV_0_RATE 1200 Bytes/sMAV_1_RATE 0 (Half maximum)MAV_0_FORWARD TrueMAV_1_FORWARD DisabledSER_TEL1_BAUD 57600SER_TEL2_BAUD 921600
详细端口设置参见https://docs.px4.io/main/en/peripherals/mavlink_peripherals.html
配置好端口后需要做一根连接机载电脑和Pixhawk TELEM 2 端口的线我这边直接用一个USB转TTL模块。 Pixhawk TELEM1 / TELEM2 端口线序图如下
PinSignalVolt1 (red)VCC5V2 (blk)TX (OUT)3.3V3 (blk)RX (IN)3.3V4 (blk)CTS (IN)3.3V5 (blk)RTS (OUT)3.3V6 (blk)GNDGND
其它端口详见https://docs.px4.io/main/en/flight_controller/pixhawk.html#where-to-buy
展示一张做好的端子连接线如下
在qgc上测试通信是否正常。
qgc-Application Settings-通讯连接-添加设置如下。 正常情况下就可通过TELEM 2连上QGC了。
二、在机载电脑上启动MAVROS
我这里用的是nuc的usb设备名称dev/ttyUSB0按照自己实际情况配置。921600是波特率就是前面设置的SER_TEL2_BAUD参数改成设置值就行。
roslaunch mavros px4.launch fcu_url:serial:/dev/ttyUSB0:921600 gcs_url:udp://172.16.7.210gcs_url运行qgc主机的IP地址。 如果不想设置ip可以设置为以下参数自动寻址。
roslaunch mavros px4.launch fcu_url:serial:/dev/ttyUSB0:921600 gcs_url:udp-b://若出现报错 FCU: DeviceError:serial:open: Permission denied
解决方法是给对应的串口权限
sudo chmod 777 /dev/ttyUSB0三、vins_fusion、mavros建立连接
思考vins_fusion的里程计数据如何发布给px4 这也是实现室内定位的关键。实现这一步只需将vins_fusion里程计数据以话题 /mavros/vision_pose/pose 发布mavros 收到/mavros/vision_pose/pose话题后转化成mavlink通过TELEM 2传给飞控。于是就完成了vins_fusion和px4的连接。
接下来创建发布 /mavros/vision_pose/pose 话题的功能包过程了
1.创建工作空间px4ctrl
mkdir -p px4ctrl/src/
cd px4ctrl/src/2.创建功能包vins_to_mavros
catkin_create_pkg vins_to_mavros roscpp std_msgs geometry_msgs mavros_msgs nav_msgs tf2_eigen tf在px4ctrl/src/vins_to_mavros/src/ 目录下创建一个 vins_to_mavros 节点主要功能 1将 VINS-Fusion 的 body 坐标系在 world 坐标系下为位姿转化为 base_link 在 map 坐标系中的位姿 2将转化后的位姿信息以话题 /mavros/vision_pose/pose 发布。
#include ros/ros.h
#include geometry_msgs/PoseStamped.h
#include nav_msgs/Odometry.h
#include Eigen/EigenEigen::Vector3d p_mav;
Eigen::Quaterniond q_mav;void vins_callback(const nav_msgs::Odometry::ConstPtr msg)
{if(msg-header.frame_id world){p_mav Eigen::Vector3d(msg-pose.pose.position.y, -msg-pose.pose.position.x, msg-pose.pose.position.z);q_mav Eigen::Quaterniond(msg-pose.pose.orientation.w, msg-pose.pose.orientation.x, msg-pose.pose.orientation.y, msg-pose.pose.orientation.z);Eigen::AngleAxisd roll(M_PI/2,Eigen::Vector3d::UnitX()); // 绕 x 轴旋转 pi / 2Eigen::AngleAxisd pitch(0,Eigen::Vector3d::UnitY());Eigen::AngleAxisd yaw(0,Eigen::Vector3d::UnitZ());Eigen::Quaterniond _q_mav roll * pitch * yaw;q_mav q_mav * _q_mav;}
}int main(int argc, char **argv)
{ros::init(argc, argv, vins_to_mavros);ros::NodeHandle nh(~);ros::Subscriber slam_sub nh.subscribenav_msgs::Odometry(odom, 100, vins_callback);ros::Publisher vision_pub nh.advertisegeometry_msgs::PoseStamped(vision_pose, 10);// the setpoint publishing rate MUST be faster than 2Hzros::Rate rate(20.0);ros::Time last_request ros::Time::now();while(ros::ok()) {geometry_msgs::PoseStamped vision;vision.pose.position.x p_mav[0];vision.pose.position.y p_mav[1];vision.pose.position.z p_mav[2];vision.pose.orientation.x q_mav.x();vision.pose.orientation.y q_mav.y();vision.pose.orientation.z q_mav.z();vision.pose.orientation.w q_mav.w();vision.header.stamp ros::Time::now();vision_pub.publish(vision);ROS_INFO(\nposition:\n x: %.18f\n y: %.18f\n z: %.18f\norientation:\n x: %.18f\n y: %.18f\n z: %.18f\n w: %.18f, \p_mav[0],p_mav[1],p_mav[2],q_mav.x(),q_mav.y(),q_mav.z(),q_mav.w());ros::spinOnce();rate.sleep();}return 0;
}
3.配置 CMakeList.txt 文件
找到相应位置添加
add_executable(vins_to_mavros_node src/vins_to_mavros.cpp)
target_link_libraries(vins_to_mavros_node ${catkin_LIBRARIES})4.创建vins_to_mavros节点的launch文件
在目录px4ctrl/src/launch/ 创建vins_to_mavros.launch
launchnode pkgvins_to_mavros typevins_to_mavros_node namevins_to_mavros outputscreenremap from~vision_pose to/mavros/vision_pose/pose /remap from~odom to/vins_estimator/odometry //node
/launch
5.编译
cd px4ctrl
catkin_make
source devel/setup.bash6.验证
启动vins_to_mavros节点
roslaunch vins_to_mavros vins_to_mavros.launch查看话题
rostopic list显示如下
PS此处坐标系转化适配的是Realsense D435i相机如其它相机需要根据相机imu坐标系与px4坐标系进行相应的转换。
四、联调测试
联调测试的基础是 1.vins_fusion里程计精度尚可且具有一定的鲁棒性 2.px4飞控在自稳模式下手动可控、达到可飞条件。
关于vins_fusion的相关调试参见系列文章 https://blog.csdn.net/u010196944/article/details/127240169
1.px4飞控设置
将px4定位数据源设置为vinsion参数EKF2_AID_MASK设置为24具体如下 2.在终端依次输入 此时qgc已连上可在qgc作如下验证。
Analyze Tools-MAVlink检测出现了LOCAL_POSITION_NED数据如下 验证 1前后左右移动飞机看看位置是否正确。 2前后移动飞机后放回原位置看位置数据偏差是否大。
验证没问题之后就可以起飞通过qgc或者遥控器切换定位模式了。 完结希望你一切顺利不“炸鸡”。
参考
1.https://blog.csdn.net/u010196944/article/details/127240169 2.https://docs.px4.io/main/en/ 3.https://zhuanlan.zhihu.com/p/364390798 4.https://blog.csdn.net/qq_44998513/article/details/133144421?spm1001.2014.3001.5502
