VINS_FUSIOＮ意義?

?VINS Fusion在VINS Mono的基礎(chǔ)上，添加了GPS等可以獲取全局觀測(cè)信息的傳感器，使得VINS可以利用全局信息消除累計(jì)誤差，進(jìn)而減小閉環(huán)依賴。此外，全局信息可以使分多次運(yùn)行的VINS Mono統(tǒng)一到一個(gè)坐標(biāo)系，從而方便協(xié)同建圖和定位。

局部傳感器（如ＩＭＵ，相機(jī)，雷達(dá)等）被廣泛應(yīng)用與建圖與定位算法。盡管這些傳感器能在沒(méi)有ＧＰＳ信息的區(qū)域，實(shí)現(xiàn)良好的局部定位和建圖效果，但這些傳感器只能提供局部觀測(cè)，限制了其應(yīng)用場(chǎng)景：

１、第一個(gè)問(wèn)題是局部觀測(cè)數(shù)據(jù)缺乏全局約束，當(dāng)我們每次在不同的位置運(yùn)行算法時(shí)，都會(huì)得到不同坐標(biāo)系下的定位和建圖結(jié)果，因而難以將這些測(cè)量結(jié)果結(jié)合起來(lái)，形成全局效果。

２、第二個(gè)問(wèn)題時(shí)基于局部觀測(cè)的估計(jì)算法必然存在累積漂移，盡管回環(huán)檢測(cè)可以在一定程度上消除漂移，但是對(duì)于數(shù)據(jù)量較大的大范圍場(chǎng)景，算法依然難以處理。

相比于局部傳感器，全局傳感器（如ＧＰＳ，氣壓計(jì)和磁力計(jì)等）可以提供全局觀測(cè)，這些傳感器使用全局統(tǒng)一坐標(biāo)系，并且輸出的觀測(cè)數(shù)據(jù)的方差不隨時(shí)間累積而增加，但這些傳感器也存在一些問(wèn)題，導(dǎo)致無(wú)法直接用于精確定位和建圖，以ＧＰＳ為例，ＧＰＳ數(shù)據(jù)通常不平滑，存在噪聲，且輸出速率低，因此，一個(gè)簡(jiǎn)單而直觀的想法是將局部傳感器和全局傳感器結(jié)合起來(lái)，以達(dá)到局部精確全局零漂的效果，也即是ＶＩＮＳ　Fusion的核心。

其算法框架如下：

?下圖以因子圖的方式表示觀測(cè)和狀態(tài)之間的約束：

其中圓形為狀態(tài)量(如位姿，速度，偏置等)，黃色正方形為局部觀測(cè)的約束，即來(lái)自VO/VIO的相對(duì)位姿變換；而其他顏色的正方形為全局觀測(cè)的約束，比如紫色正方形為來(lái)自GPS的約束。

局部約束(殘差)的構(gòu)建參考vins mono論文，計(jì)算的是相鄰兩幀之間的位姿殘差。這里只討論GPS帶來(lái)的全局約束。首先當(dāng)然是將GPS坐標(biāo)，也就是經(jīng)緯度轉(zhuǎn)換為大地坐標(biāo)系。習(xí)慣上選擇右手坐標(biāo)系，x,y,z軸正方向分別是北東地或東北天方向。接下來(lái)就可以計(jì)算得到全局約束的殘差：

其中z為GPS測(cè)量值，X為狀態(tài)預(yù)測(cè)，h方程為觀測(cè)方程。X可以通過(guò)上一時(shí)刻狀態(tài)以及當(dāng)前時(shí)刻與上一時(shí)刻的位姿變換計(jì)算出來(lái)。具體到本文的方法，就是利用VIO得到當(dāng)前時(shí)刻和上一時(shí)刻的相對(duì)位姿dX，加到上一時(shí)刻的位姿上X(i-1)，得到當(dāng)前時(shí)刻的位姿Xi。需要注意的是，這里的X以第一幀為原點(diǎn)。通過(guò)觀測(cè)方程h，將當(dāng)前狀態(tài)的坐標(biāo)轉(zhuǎn)換到GPS坐標(biāo)系下。這樣就構(gòu)建了一個(gè)全局約束。

之后的優(yōu)化就交給BA優(yōu)化器進(jìn)行迭代優(yōu)化，vins fusion沿用了ceres作為優(yōu)化器。

代碼解析

1. GPS和VIO數(shù)據(jù)輸入

需要明確的一點(diǎn)是，VIO的輸出是相對(duì)于第一幀的累計(jì)位姿變換，也就是以第一幀為原點(diǎn)。VINS Fusion接收vio輸出的局部位姿變換(相對(duì)于第一幀)，以及gps輸出的經(jīng)緯度坐標(biāo)，進(jìn)行融合。接受數(shù)據(jù)輸入的接口在global_fusion/src/globaOptNode.cpp文件中，接口定義的關(guān)鍵代碼如下：

/******************************************************** Copyright (C) 2019, Aerial Robotics Group, Hong Kong University of Science and Technology* * This file is part of VINS.* * Licensed under the GNU General Public License v3.0;* you may not use this file except in compliance with the License.** Author: Qin Tong (qintonguav@gmail.com)GPS和VIO數(shù)據(jù)輸入*******************************************************/#include "ros/ros.h" #include "globalOpt.h" #include <sensor_msgs/NavSatFix.h> #include <nav_msgs/Odometry.h> #include <nav_msgs/Path.h> #include <eigen3/Eigen/Dense> #include <eigen3/Eigen/Geometry> #include <iostream> #include <stdio.h> #include <visualization_msgs/Marker.h> #include <visualization_msgs/MarkerArray.h> #include <fstream> #include <queue> #include <mutex>GlobalOptimization globalEstimator; ros::Publisher pub_global_odometry, pub_global_path, pub_car; nav_msgs::Path *global_path; double last_vio_t = -1; std::queue<sensor_msgs::NavSatFixConstPtr> gpsQueue; std::mutex m_buf;void publish_car_model(double t, Eigen::Vector3d t_w_car, Eigen::Quaterniond q_w_car) {visualization_msgs::MarkerArray markerArray_msg;visualization_msgs::Marker car_mesh;car_mesh.header.stamp = ros::Time(t);car_mesh.header.frame_id = "world";car_mesh.type = visualization_msgs::Marker::MESH_RESOURCE;car_mesh.action = visualization_msgs::Marker::ADD;car_mesh.id = 0;car_mesh.mesh_resource = "package://global_fusion/models/car.dae";Eigen::Matrix3d rot;rot << 0, 0, -1, 0, -1, 0, -1, 0, 0;Eigen::Quaterniond Q;Q = q_w_car * rot; car_mesh.pose.position.x = t_w_car.x();car_mesh.pose.position.y = t_w_car.y();car_mesh.pose.position.z = t_w_car.z();car_mesh.pose.orientation.w = Q.w();car_mesh.pose.orientation.x = Q.x();car_mesh.pose.orientation.y = Q.y();car_mesh.pose.orientation.z = Q.z();car_mesh.color.a = 1.0;car_mesh.color.r = 1.0;car_mesh.color.g = 0.0;car_mesh.color.b = 0.0;float major_scale = 2.0;car_mesh.scale.x = major_scale;car_mesh.scale.y = major_scale;car_mesh.scale.z = major_scale;markerArray_msg.markers.push_back(car_mesh);pub_car.publish(markerArray_msg); }void GPS_callback(const sensor_msgs::NavSatFixConstPtr &GPS_msg) {//printf("gps_callback! \n");m_buf.lock();gpsQueue.push(GPS_msg); //每次接收到的gps消息添加到gpsqueue隊(duì)列中m_buf.unlock(); }void vio_callback(const nav_msgs::Odometry::ConstPtr &pose_msg) {//printf("vio_callback! \n");double t = pose_msg->header.stamp.toSec();last_vio_t = t;Eigen::Vector3d vio_t(pose_msg->pose.pose.position.x, pose_msg->pose.pose.position.y, pose_msg->pose.pose.position.z);//平移矩陣Eigen::Quaterniond vio_q; //旋轉(zhuǎn)四元陣vio_q.w() = pose_msg->pose.pose.orientation.w;vio_q.x() = pose_msg->pose.pose.orientation.x;vio_q.y() = pose_msg->pose.pose.orientation.y;vio_q.z() = pose_msg->pose.pose.orientation.z;globalEstimator.inputOdom(t, vio_t, vio_q);//將時(shí)間、平移，四元數(shù)作為預(yù)測(cè)添加到globalEstimatorm_buf.lock();while(!gpsQueue.empty()){sensor_msgs::NavSatFixConstPtr GPS_msg = gpsQueue.front();double gps_t = GPS_msg->header.stamp.toSec();printf("vio t: %f, gps t: %f \n", t, gps_t);// 10ms sync tolerance 找到vio里程數(shù)據(jù)相差在10ms之內(nèi)的數(shù)據(jù)，作為匹配數(shù)據(jù)if(gps_t >= t - 0.01 && gps_t <= t + 0.01){//printf("receive GPS with timestamp %f\n", GPS_msg->header.stamp.toSec());double latitude = GPS_msg->latitude;double longitude = GPS_msg->longitude;double altitude = GPS_msg->altitude;//int numSats = GPS_msg->status.service;double pos_accuracy = GPS_msg->position_covariance[0];if(pos_accuracy <= 0)pos_accuracy = 1;//printf("receive covariance %lf \n", pos_accuracy);//if(GPS_msg->status.status > 8)globalEstimator.inputGPS(t, latitude, longitude, altitude, pos_accuracy);//關(guān)鍵，將gps數(shù)據(jù)作為觀測(cè)輸入到globalEstimator中g(shù)psQueue.pop(); //滿足條件的gps信息彈出break;}else if(gps_t < t - 0.01)gpsQueue.pop();//過(guò)時(shí)的gps消息彈出else if(gps_t > t + 0.01)break;//說(shuō)明gps消息是后來(lái)的，就不要改gps隊(duì)列，退出}m_buf.unlock();Eigen::Vector3d global_t;Eigen:: Quaterniond global_q;globalEstimator.getGlobalOdom(global_t, global_q);nav_msgs::Odometry odometry;odometry.header = pose_msg->header;odometry.header.frame_id = "world";odometry.child_frame_id = "world";odometry.pose.pose.position.x = global_t.x();odometry.pose.pose.position.y = global_t.y();odometry.pose.pose.position.z = global_t.z();odometry.pose.pose.orientation.x = global_q.x();odometry.pose.pose.orientation.y = global_q.y();odometry.pose.pose.orientation.z = global_q.z();odometry.pose.pose.orientation.w = global_q.w();pub_global_odometry.publish(odometry);pub_global_path.publish(*global_path);publish_car_model(t, global_t, global_q);// write result to filestd::ofstream foutC("/home/tony-ws1/output/vio_global.csv", ios::app);foutC.setf(ios::fixed, ios::floatfield);foutC.precision(0);foutC << pose_msg->header.stamp.toSec() * 1e9 << ",";foutC.precision(5);foutC << global_t.x() << ","<< global_t.y() << ","<< global_t.z() << ","<< global_q.w() << ","<< global_q.x() << ","<< global_q.y() << ","<< global_q.z() << endl;foutC.close(); }int main(int argc, char **argv) {ros::init(argc, argv, "globalEstimator");ros::NodeHandle n("~");global_path = &globalEstimator.global_path;ros::Subscriber sub_GPS = n.subscribe("/gps", 100, GPS_callback);ros::Subscriber sub_vio = n.subscribe("/vins_estimator/odometry", 100, vio_callback);pub_global_path = n.advertise<nav_msgs::Path>("global_path", 100);pub_global_odometry = n.advertise<nav_msgs::Odometry>("global_odometry", 100);pub_car = n.advertise<visualization_msgs::MarkerArray>("car_model", 1000);ros::spin();return 0; }

2. GPS和VIO融合

VINS Fusion融合GPS和VIO數(shù)據(jù)的代碼在global_fusion/src/globalOpt.cpp文件中，下面進(jìn)行詳細(xì)介紹。

a. 接收GPS數(shù)據(jù)，接收VIO數(shù)據(jù)并轉(zhuǎn)到GPS坐標(biāo)系

// 接收上面輸入的vio數(shù)據(jù) void GlobalOptimization::inputOdom(double t, Eigen::Vector3d OdomP, Eigen::Quaterniond OdomQ){vector<double> localPose{OdomP.x(), OdomP.y(), OdomP.z(),OdomQ.w(), OdomQ.x(), OdomQ.y(), OdomQ.z()};localPoseMap[t] = localPose;// 利用vio的局部坐標(biāo)進(jìn)行坐標(biāo)變換，得到當(dāng)前幀的全局位姿Eigen::Quaterniond globalQ;globalQ = WGPS_T_WVIO.block<3, 3>(0, 0) * OdomQ;Eigen::Vector3d globalP = WGPS_T_WVIO.block<3, 3>(0, 0) * OdomP + WGPS_T_WVIO.block<3, 1>(0, 3);vector<double> globalPose{globalP.x(), globalP.y(), globalP.z(),globalQ.w(), globalQ.x(), globalQ.y(), globalQ.z()};globalPoseMap[t] = globalPose; }// 接收上面輸入的gps數(shù)據(jù) void GlobalOptimization::inputGPS(double t, double latitude, double longitude, double altitude, double posAccuracy) {double xyz[3];GPS2XYZ(latitude, longitude, altitude, xyz); // 將GPS的經(jīng)緯度轉(zhuǎn)到地面笛卡爾坐標(biāo)系vector<double> tmp{xyz[0], xyz[1], xyz[2], posAccuracy};GPSPositionMap[t] = tmp;newGPS = true; }

上面出現(xiàn)了VIO數(shù)據(jù)的局部坐標(biāo)轉(zhuǎn)到GPS坐標(biāo)的計(jì)算過(guò)程，其公式如下：

這個(gè)公式中的GPS2VIO出現(xiàn)在后面的優(yōu)化過(guò)程中，計(jì)算方法為：

b. 融合優(yōu)化

這里是融合的關(guān)鍵代碼，可以看出其流程如下：

構(gòu)建t_array和q_array，用來(lái)存入平移和旋轉(zhuǎn)變量，方便輸入優(yōu)化方程，以及在優(yōu)化后取出。

利用RelativeRTError::Create()構(gòu)建VIO兩幀之間的約束，輸入優(yōu)化方程

利用TError::Create()構(gòu)建GPS構(gòu)成的全局約束，輸入優(yōu)化方程

取出優(yōu)化后的數(shù)據(jù)

void GlobalOptimization::optimize() {while(true){if(newGPS){newGPS = false;// ceres定義部分略去// add parammPoseMap.lock();int length = localPoseMap.size();// ********************************************************// *** 1. 構(gòu)建t_array, q_array用來(lái)存優(yōu)化變量，等優(yōu)化后取出 ***// ********************************************************double t_array[length][3];double q_array[length][4];map<double, vector<double>>::iterator iter;iter = globalPoseMap.begin();for (int i = 0; i < length; i++, iter++){// 取出數(shù)據(jù)部分省略// 添加了parameterblockproblem.AddParameterBlock(q_array[i], 4, local_parameterization);problem.AddParameterBlock(t_array[i], 3);}map<double, vector<double>>::iterator iterVIO, iterVIONext, iterGPS;int i = 0;for (iterVIO = localPoseMap.begin(); iterVIO != localPoseMap.end(); iterVIO++, i++){// ********************************************************// ********************* 2. VIO約束 *******************// ********************************************************iterVIONext = iterVIO;iterVIONext++;if(iterVIONext != localPoseMap.end()){Eigen::Matrix4d wTi = Eigen::Matrix4d::Identity(); // 第i幀的變換矩陣Eigen::Matrix4d wTj = Eigen::Matrix4d::Identity(); // 第j幀的變換矩陣// 取出數(shù)據(jù)部分省略Eigen::Matrix4d iTj = wTi.inverse() * wTj; // 第j幀到第i幀的變換矩陣Eigen::Quaterniond iQj; // 第j幀到第i幀的旋轉(zhuǎn)iQj = iTj.block<3, 3>(0, 0);Eigen::Vector3d iPj = iTj.block<3, 1>(0, 3); // 第j幀到第i幀的平移ceres::CostFunction* vio_function = RelativeRTError::Create(iPj.x(), iPj.y(), iPj.z(),iQj.w(), iQj.x(), iQj.y(), iQj.z(),0.1, 0.01);problem.AddResidualBlock(vio_function, NULL, q_array[i], t_array[i], q_array[i+1], t_array[i+1]);}// ********************************************************// ********************* 3. GPS約束 *******************// ********************************************************double t = iterVIO->first;iterGPS = GPSPositionMap.find(t);if (iterGPS != GPSPositionMap.end()){ceres::CostFunction* gps_function = TError::Create(iterGPS->second[0], iterGPS->second[1],iterGPS->second[2], iterGPS->second[3]);problem.AddResidualBlock(gps_function, loss_function, t_array[i]);}}ceres::Solve(options, &problem, &summary);// ********************************************************// ******************* 4. 取出優(yōu)化結(jié)果 *****************// ********************************************************iter = globalPoseMap.begin();for (int i = 0; i < length; i++, iter++){// 取出優(yōu)化結(jié)果vector<double> globalPose{t_array[i][0], t_array[i][1], t_array[i][2],q_array[i][0], q_array[i][1], q_array[i][2], q_array[i][3]};iter->second = globalPose;if(i == length - 1){Eigen::Matrix4d WVIO_T_body = Eigen::Matrix4d::Identity();Eigen::Matrix4d WGPS_T_body = Eigen::Matrix4d::Identity();// 剩余的存入部分省略，得到WGPS_T_WVIOWGPS_T_WVIO = WGPS_T_body * WVIO_T_body.inverse();}}updateGlobalPath();mPoseMap.unlock();}std::chrono::milliseconds dura(2000);std::this_thread::sleep_for(dura);}return; }

上述代碼中出現(xiàn)了一個(gè)關(guān)鍵的部分，即WGPS_T_WVIO的計(jì)算。從之前的代碼中知道，這個(gè)4*4矩陣是用來(lái)做VIO到GPS坐標(biāo)系的變換的。按道理講，這個(gè)矩陣應(yīng)當(dāng)是不需要反復(fù)計(jì)算的，畢竟第一幀和GPS的坐標(biāo)變換是確定的。但是運(yùn)行結(jié)果來(lái)看，這個(gè)矩陣的值是在變化的，而且有時(shí)變化比較大。這里不太懂，知乎劉知說(shuō)是為了避免VIO漂移過(guò)大。

3. 總結(jié)(摘自知乎劉知)

使用場(chǎng)景

根據(jù)上文中分析的優(yōu)化策略，global fusion的應(yīng)用場(chǎng)景應(yīng)該是GPS頻率較低，VIO頻率較高的系統(tǒng)。fusion 默認(rèn)發(fā)布頻率位10hz，而現(xiàn)在的GPS可以達(dá)到20hz，如果在這種系統(tǒng)上使用，你可能還需要修改下VIO或者GPS頻率。另外，使用的GPS是常見的誤差較大的GPS，并不是RTK-GPS。

? ? 2. GPS與VIO時(shí)間不同步

上文提到了，在多傳感器融合系統(tǒng)中，傳感器往往需要做時(shí)鐘同步，那么global Fusion需要么？GPS分為為很多種，我們常見的GPS模塊精度較低，十幾米甚至幾十米的誤差，這種情況下，同不同步?jīng)]那么重要了，因?yàn)镚PS方差太大。另外一種比較常見的是RTK-GPS ，在無(wú)遮擋的情況下，室外精度可以達(dá)到 2cm之內(nèi)，輸出頻率可以達(dá)到20hz，這種情況下，不同步時(shí)間戳?xí)?duì)系統(tǒng)產(chǎn)生影響，如果VIO要和RTK做松耦合，這點(diǎn)還需要注意。

三、前后端詳解

總結(jié)

以上是生活随笔為你收集整理的VINS_FUSION的全部?jī)?nèi)容，希望文章能夠幫你解決所遇到的問(wèn)題。

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯(cuò)，歡迎將生活随笔推薦給好友。