[学习记录]realsence d455 +vins-fusion+px4+ego_planner下无人机的悬停与控制

写在前面：持续更新修改......

my env：ubuntu20

my pixhawk：2.4.8

my px4 firmware：1.9.0 stablepx4fmu-v2-1.6.0.px4v

经济型科研方案（有个较好的网络环境）：

无人机端：250机架——38块钱或者可以更便宜、三叶桨——也很便宜、飞控——2.4.8——五百多、机载板子——jetsonnano（可以选择更便宜但是wifi模块还是要强）、电池也很便宜。

地面站：电脑一台

无人机端的机载板子主要用于通信，传输数据，全部算法都在电脑上跑（手动滑稽）。

关于带宽问题，使用压缩图即可

关于网络出错等问题，几乎都可以通过修改dns与/etc/hosts解决。

-------------------------------------------------------------分割符-----------------------------------------------------

整体步骤整理，整体的步骤来自（注意看里面的readme.txt）：https://github.com/ZJU-FAST-Lab/Fast-Drone-250https://github.com/ZJU-FAST-Lab/Fast-Drone-250

Step1: 安装 realsence2 驱动，把ros自带的和源码都安装了，并检测和imu数据与camera数据。

参考GitHub - IntelRealSense/realsense-ros: Intel(R) RealSense(TM) ROS Wrapper for D400 series, SR300 Camera and T265 Tracking ModuleIntel(R) RealSense(TM) ROS Wrapper for D400 series, SR300 Camera and T265 Tracking Module - GitHub - IntelRealSense/realsense-ros: Intel(R) RealSense(TM) ROS Wrapper for D400 series, SR300 Camera and T265 Tracking Modulehttps://github.com/IntelRealSense/realsense-ros

Step2：安装vins-fustion，并测试结果

参考GitHub - HKUST-Aerial-Robotics/VINS-Fusion: An optimization-based multi-sensor state estimatorhttps://github.com/HKUST-Aerial-Robotics/VINS-Fusion

如果遇到一些问题还能参考：＜解决方法＞ubuntu20+ros+opencv4运行vinfusion_fikimi的博客-CSDN博客

Step3：仿真环境安装，并解决px4 和机载板卡与笔记本的实机通信问题，连接方式等

ubuntu18+jetson nano +px4+ros ＜——＞QGC+ubuntu20+ros(关于仿真和实物运行的持续记录心得)_fikimi的博客-CSDN博客

notice：关于在px4 的sd卡写入marlink 命令或者QGC的mavlink控制台修改帧率问题：通过telem2 连接，硬件写/dev/ttyS2,通过telem1 连接，写/dev/ttyS1,通过usb 则写/dev/ttyACM0

mavlink stream -d /dev/ttyS2 -s ATTITUDE_QUATERNION -r 200
mavlink stream -d /dev/ttyS2 -s HIGHRES_IMU -r 200

Step4：（这里可以写个shell，省得一条一条按，关于shell运行ssh的方法可以查expect的用法）

drone0-terminal1：roslaunch px4_realsense_bridge mypx4.launch

drone0-terminal2：roslaunch realsense2_camera rs_camera.launch

ubuntu-terminal1：roscore

ubuntu-terminal2：roslaunch vins vins_fusion.launch

ubuntu-terminal3：roslaunch vins rviz.launch

ubuntu-terminal4：roslaunch px4ctrl run_ctrl.launch

ubuntu-terminal5：sh shfiles/takeoff.sh

其中，在遥控器（飞行模式上是通道5,都设置为自稳模式）上将模式转为Autohover(通道5)模式，，通道6拨动一下（根据遥控器可以进行修改），然后将油门推到中位，再在ubuntu-terminal5：中运行sh shfiles/takeoff.sh。

注意：Fast-lab的Fast-drone-250中包px4ctrl的readme.txt中需要详细阅读

-------------------------------------------------------------分割符-----------------------------------------------------------

（控制方法：除了使用px4ctrl，还可以借鉴px4 vio的官方的方法（或者把官方vio中的odom用vins-fusion替代注意话题的对应，tf树的转换）rosrun rqt_tf_tree rqt_tf_tree ）

controller.cpp部分代码注释（有误可以指出并交流）：

//推力估计模型，计算thr2acc_
//输入参数：估计的加速度
bool
LinearControl::estimateThrustModel(const Eigen::Vector3d &est_a,const Parameter_t &param)
{ros::Time t_now = ros::Time::now();while (timed_thrust_.size() >= 1){// Choose data before 35~45ms agostd::pair<ros::Time, double> t_t = timed_thrust_.front();double time_passed = (t_now - t_t.first).toSec();if (time_passed > 0.045) // 45ms{// printf("continue, time_passed=%f\n", time_passed);timed_thrust_.pop();continue;}if (time_passed < 0.035) // 35ms{// printf("skip, time_passed=%f\n", time_passed);return false;}/***********************************************************//* Recursive least squares algorithm with vanishing memory *//***********************************************************/double thr = t_t.second;//取出后就弹出timed_thrust_.pop();//非精准推理模型:thr2acc = est_a(2)/thr/***********************************//* Model: est_a(2) = thr2acc_ * thr *//***********************************///kLQR，you can print it to seedouble gamma = 1 / (rho2_ + thr * P_ * thr);double K = gamma * P_ * thr;thr2acc_ = thr2acc_ + K * (est_a(2) - thr * thr2acc_);P_ = (1 - K * thr) * P_ / rho2_;//printf("%6.3f,%6.3f,%6.3f,%6.3f\n", thr2acc_, gamma, K, P_);//fflush(stdout);// debug_msg_.thr2acc = thr2acc_;return true;}return false;
}
/* compute u.thrust and u.q, controller gains and other parameters are in param_ u就是q和thrust等等，根据输入的odom和imu信息计算udebug
*/
quadrotor_msgs::Px4ctrlDebug
LinearControl::calculateControl(const Desired_State_t &des,const Odom_Data_t &odom,const Imu_Data_t &imu, Controller_Output_t &u)
{/* WRITE YOUR CODE HERE *///compute disired accelerationEigen::Vector3d des_acc(0.0, 0.0, 0.0);Eigen::Vector3d Kp,Kv;//增益，pid的p，比例增益Kp << param_.gain.Kp0, param_.gain.Kp1, param_.gain.Kp2;//先把参数中的增益加进去Kv << param_.gain.Kv0, param_.gain.Kv1, param_.gain.Kv2;//输入偏差x常数des_acc = des.a + Kv.asDiagonal() * (des.v - odom.v) + Kp.asDiagonal() * (des.p - odom.p);//asDiagonal对角化伪表达//增量des_acc += Eigen::Vector3d(0,0,param_.gra);//推力计算转换，z方向加速度占比计算u.thrust = computeDesiredCollectiveThrustSignal(des_acc);double roll,pitch,yaw,yaw_imu;double yaw_odom = fromQuaternion2yaw(odom.q);//根据四元数计算出欧拉角，ros的odom消息机制为ZYX，无人机的是ZXYdouble sin = std::sin(yaw_odom);double cos = std::cos(yaw_odom);//跟着把roll yaw pitch 算出来roll = (des_acc(0) * sin - des_acc(1) * cos )/ param_.gra;pitch = (des_acc(0) * cos + des_acc(1) * sin )/ param_.gra;//yaw = fromQuaternion2yaw(des.q);yaw_imu = fromQuaternion2yaw(imu.q);//一个道理，把imu的yaw算出来// Eigen::Quaterniond q = Eigen::AngleAxisd(yaw,Eigen::Vector3d::UnitZ())//   * Eigen::AngleAxisd(roll,Eigen::Vector3d::UnitX())//   * Eigen::AngleAxisd(pitch,Eigen::Vector3d::UnitY());//旋转向量（AngleAxisd）初始化，des为旋转角，Eigen::Vector3d::UnitZ()表示绕z轴转.Z-Y-X//odom.q 接受的话题是Z-Y-X //odom.q.inverse()表示 odom.q的共轭，并且单位化Eigen::Quaterniond q = Eigen::AngleAxisd(des.yaw,Eigen::Vector3d::UnitZ())* Eigen::AngleAxisd(pitch,Eigen::Vector3d::UnitY())* Eigen::AngleAxisd(roll,Eigen::Vector3d::UnitX());u.q = imu.q * odom.q.inverse() * q;// Align with FCU frame/* WRITE YOUR CODE HERE *///used for debug。可以自己添加debug_msg_.des_p_x = des.p(0);debug_msg_.des_p_y = des.p(1);debug_msg_.des_p_z = des.p(2);debug_msg_.des_v_x = des.v(0);debug_msg_.des_v_y = des.v(1);debug_msg_.des_v_z = des.v(2);debug_msg_.des_a_x = des_acc(0);debug_msg_.des_a_y = des_acc(1);debug_msg_.des_a_z = des_acc(2);debug_msg_.des_q_x = u.q.x();debug_msg_.des_q_y = u.q.y();debug_msg_.des_q_z = u.q.z();debug_msg_.des_q_w = u.q.w();debug_msg_.des_thr = u.thrust;// Used for thrust-accel mapping estimation 推力加速度映射估计timed_thrust_.push(std::pair<ros::Time, double>(ros::Time::now(), u.thrust));while (timed_thrust_.size() > 100){timed_thrust_.pop();}return debug_msg_;
}

-----------------------------------------------------------------分割符--------------------------------------------------------

[学习记录]realsence d455 +vins-fusion+px4+ego_planner下无人机的悬停与控制相关推荐

Linux 学习记录之——Linux简介及Vmware 虚拟机下分区和centos6系统安装
课程大纲: 一.UNIX 与Linux发展史: 1.比拟说,Unix是父亲,Linux是儿子. 2.Unix是六十年代中期,Linux80年代, Windows 90年代. 3.miutix 大而复杂 ...
基于PX4六旋翼无人机百米悬停定点降落
LRS+XAPI教育学习记录（安装lrs，以及用xapi传输数据到lrs）
此LRS團隊出了最新版的LRS,是node寫的.運行速度是本文寫的這個版本的無數倍,待我整理好再發出來. 关于lrs跟xapi国内基本用的少,质料基本都是英文的,中文的也就只有繁体的一些少少的资料.新 ...
VINS Fusion GPS融合部分
概述 VINS Fusion在VINS Mono的基础上,添加了GPS等可以获取全局观测信息的传感器,使得VINS可以利用全局信息消除累计误差,进而减小闭环依赖. 局部传感器(如相机,IMU,激光雷达 ...
Cooperative Perception协同感知学习记录
Cooperative Perception协同感知学习记录文章目录 Cooperative Perception协同感知学习记录 1. 首先介绍一篇不错的Revision文章:Deep Multi ...
小觅相机D系列跑vins fusion
有幸使用一款D系列小觅相机.今天测试了下它跑vins fusion的表现.其中有一个注意点,官方文档没有说明.这里记录一下. D系列小觅相机添加了IR散斑投射器,用来恢复没有纹理细节时的深度.这与老版 ...
Pytorch学习记录-torchtext和Pytorch的实例（使用神经网络训练Seq2Seq代码）
Pytorch学习记录-torchtext和Pytorch的实例1 0. PyTorch Seq2Seq项目介绍 1. 使用神经网络训练Seq2Seq 1.1 简介,对论文中公式的解读 1.2 数据预 ...
HTML5与CSS3权威指南之CSS3学习记录
title: HTML5与CSS3权威指南之CSS3学习记录 toc: true date: 2018-10-14 00:06:09 学习资料--<HTML5与CSS3权威指南>(第3版) ...
springboot @cacheable不起作用_Springboot学习记录13 使用缓存：整合redis
本学习记录的代码,部分参考自gitee码云的如下工程.这个工程有详尽的Spingboot1.x教程.鸣谢! https://gitee.com/didispace/SpringBoot-Learnin ...

[学习记录]realsence d455 +vins-fusion+px4+ego_planner下无人机的悬停与控制

[学习记录]realsence d455 +vins-fusion+px4+ego_planner下无人机的悬停与控制相关推荐

最新文章

热门文章