目录
- 前言
- 环境配置
- 运行fast-lio
- 修改px4位置信息融合方式
- 编写位置坐标转换及传输节点
前言
在配置mid360运行环境后，可使用mid360进行室内的精准定位。
环境配置
在livox_ros_driver2的上级目录src下保存fast-lio的工程
git clone https://github.com/hku-mars/FAST_LIO.git
cd FAST_LIO
git submodule update --init
为使用mid360作为硬件输入修改源代码中的所有livox_ros_driver为livox_ros_driver2（包括.cpp .h 以及 package.xml）
在livox_ros_driver2的pkg中编译
cd src/livox_ros_driver2/
./build ROS1
编译过程大概需要3g的内存，若机载板物理内存不足，需要增大swap大小增加交换空间，可参考增加swap解决。
运行fast-lio
执行下述指令时请确保mid360运行环境中的rviz可以成功显示环境点云信息。
执行以下指令
roslaunch livox_ros_driver2 msg_MID360.launch
在另一个终端中执行
roslaunch fast_lio mapping_mid360.launch
执行后使用rostopic list查看话题列表，出现/Odometry话题即为成功运行
使用
rostopic echo /Odometry
可以查看当前的定位定姿信息。
修改px4位置信息融合方式
这里使用光流以及激光定位信息。
修改EKF2_AID_MASK为10
编写位置坐标转换及传输节点
使用/mavros/vision_pose/pose话题将激光得到的定位信息传递至px4进行融合，需注意该话题的位置信息应建立在ENU坐标系下（MAVROS使用该坐标系作为惯性系），传递至px4接收时会自动转化为NED坐标系供EKF2进行融合。
因此需首先计算出初始化时fast-lio所产生的坐标系与ENU坐标系的旋转关系（主要为偏航角），并将该转换关系定为初始值
init_q = Eigen::AngleAxisd(init_yaw,Eigen::Vector3d::UnitZ())//des.yaw
* Eigen::AngleAxisd(0.0,Eigen::Vector3d::UnitY())
* Eigen::AngleAxisd(0.0,Eigen::Vector3d::UnitX());
为减小初始偏航角误差，使用滑动窗口求平均值。
class SlidingWindowAverage {
public:
SlidingWindowAverage(int windowSize) : windowSize(windowSize), windowSum(0.0) {}
double addData(double newData) {
if(!dataQueue.empty()&&fabs(newData-dataQueue.back())>0.01){
dataQueue = std::queue<double>();
windowSum = 0.0;
dataQueue.push(newData);
windowSum += newData;
}
else{
dataQueue.push(newData);
windowSum += newData;
}
// 如果队列大小超过窗口大小，弹出队列头部元素并更新窗口和队列和
if (dataQueue.size() > windowSize) {
windowSum -= dataQueue.front();
dataQueue.pop();
}
windowAvg = windowSum / dataQueue.size();
// 返回当前窗口内的平均值
return windowAvg;
}
int get_size(){
return dataQueue.size();
}
double get_avg(){
return windowAvg;
}
private:
int windowSize;
double windowSum;
double windowAvg;
std::queue<double> dataQueue;
};
求解得到较为准确的初始偏航角后，该偏航角可视为fast-lio位置信息所在坐标系与惯性系的旋转关系。
在不考虑机体中心与激光雷达中心位置平动的情况下，可以将位置信息直接进行坐标转换。
p_enu = init_q*p_lidar_body;
将转换后的位置信息通过/mavros/vision_pose/pose传递
vision.pose.position.x = p_enu[0];
vision.pose.position.y = p_enu[1];
vision.pose.position.z = p_enu[2];
vision.pose.orientation.x = q_mav.x();
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);
分别执行以下节点
在QGC中可以查看LOCAL_POSITION_NED观察定位结果，静止时定位信息在3厘米以内漂移。
在调整好飞行时位置控制内外环的情况下，可以遥控起飞后切换至position模式，可以实现定点悬停。
位置转换的源码如下
#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <nav_msgs/Odometry.h>
#include <Eigen/Eigen>
#include<cmath>
#include <queue>
Eigen::Vector3d p_lidar_body, p_enu;
Eigen::Quaterniond q_mav;
Eigen::Quaterniond q_px4_odom;
class SlidingWindowAverage {
public:
SlidingWindowAverage(int windowSize) : windowSize(windowSize), windowSum(0.0) {}
double addData(double newData) {
if(!dataQueue.empty()&&fabs(newData-dataQueue.back())>0.01){
dataQueue = std::queue<double>();
windowSum = 0.0;
dataQueue.push(newData);
windowSum += newData;
}
else{
dataQueue.push(newData);
windowSum += newData;
}
// 如果队列大小超过窗口大小，弹出队列头部元素并更新窗口和队列和
if (dataQueue.size() > windowSize) {
windowSum -= dataQueue.front();
dataQueue.pop();
}
windowAvg = windowSum / dataQueue.size();
// 返回当前窗口内的平均值
return windowAvg;
}
int get_size(){
return dataQueue.size();
}
double get_avg(){
return windowAvg;
}
private:
int windowSize;
double windowSum;
double windowAvg;
std::queue<double> dataQueue;
};
int windowSize = 8;
SlidingWindowAverage swa=SlidingWindowAverage(windowSize);
double fromQuaternion2yaw(Eigen::Quaterniond q)
{
double yaw = atan2(2 * (q.x()*q.y() + q.w()*q.z()), q.w()*q.w() + q.x()*q.x() - q.y()*q.y() - q.z()*q.z());
return yaw;
}
void vins_callback(const nav_msgs::Odometry::ConstPtr &msg)
{
p_lidar_body = Eigen::Vector3d(msg->pose.pose.position.x, msg->pose.pose.position.y, 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);
}
void px4_odom_callback(const nav_msgs::Odometry::ConstPtr &msg)
{
q_px4_odom = Eigen::Quaterniond(msg->pose.pose.orientation.w, msg->pose.pose.orientation.x, msg->pose.pose.orientation.y, msg->pose.pose.orientation.z);
swa.addData(fromQuaternion2yaw(q_px4_odom));
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "vins_to_mavros");
ros::NodeHandle nh("~");
ros::Subscriber slam_sub = nh.subscribe<nav_msgs::Odometry>("/Odometry", 100, vins_callback);
ros::Subscriber px4_odom_sub = nh.subscribe<nav_msgs::Odometry>("/mavros/local_position/odom", 5, px4_odom_callback);
ros::Publisher vision_pub = nh.advertise<geometry_msgs::PoseStamped>("/mavros/vision_pose/pose", 10);
// the setpoint publishing rate MUST be faster than 2Hz
ros::Rate rate(20.0);
ros::Time last_request = ros::Time::now();
float init_yaw = 0.0;
bool init_flag = 0;
Eigen::Quaterniond init_q;
while(ros::ok()){
if(swa.get_size()==windowSize&&!init_flag){
init_yaw = swa.get_avg();
init_flag = 1;
init_q = Eigen::AngleAxisd(init_yaw,Eigen::Vector3d::UnitZ())//des.yaw
* Eigen::AngleAxisd(0.0,Eigen::Vector3d::UnitY())
* Eigen::AngleAxisd(0.0,Eigen::Vector3d::UnitX());
// delete swa;
}
if(init_flag){
geometry_msgs::PoseStamped vision;
p_enu = init_q*p_lidar_body;
vision.pose.position.x = p_enu[0];
vision.pose.position.y = p_enu[1];
vision.pose.position.z = p_enu[2];
vision.pose.orientation.x = q_mav.x();
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 in enu:\n x: %.18f\n y: %.18f\n z: %.18f\norientation of lidar:\n x: %.18f\n y: %.18f\n z: %.18f\n w: %.18f", \
p_enu[0],p_enu[1],p_enu[2],q_mav.x(),q_mav.y(),q_mav.z(),q_mav.w());
}
ros::spinOnce();
rate.sleep();
}
return 0;
}
