VINS_FUSION的global融合思想
VINS_FUSION的global融合思想
文章目录
- VINS_FUSION的global融合思想
- 使用全局融合的原因
- GPS的缺点
- 融合的目的
- 算法架构
- 观测和状态约束关系
- GPS 融合思路
- GPS残差计算
- 代码段
- 參考文献
使用全局融合的原因
- 局部观测存在累计漂移,室外大场景难以运行
- 局部观测数据缺乏全局约束,导致在不同的位置进行建图和定位都会得到不同坐标系下的结果,难以进行全局结果的观测
GPS的缺点
- 数据不平滑,噪声过大
- 输出频率低(1hz)
融合的目的
- 局部精确
- 全局零漂
算法架构
观测和状态约束关系
- state: 状态量,位姿,速度,bias等
- Local Factor: 局部观测约束,VIO相对位姿变换, 计算的是相邻两帧之间位姿的残差
GPS 融合思路
- 经纬度坐标系转换为大地坐标系(东北天坐标系ENU:local East North Up)
- 第一帧的gps点为坐标系原点
- 求最小二乘
X ∗ = arg max x ∏ t = 0 n ∏ k ∈ S e x p ( − 1 2 ∥ z t k − h t k ( X ) ) ∥ Ω t k 2 ) = arg min x ∑ t = 0 n ∑ k ∈ S ∥ z t k − h t k ( X ) ) ∥ Ω t k 2 . \begin{aligned}X^* &= \mathop{\arg\max}_{x} \ \ \prod_{t=0}^{n}\prod_{k\in S}exp(-\frac{1}{2} \|z_t^k-h_t^k(X))\|^2_{\Omega_t^k}) \\ &= \mathop{\arg\min}_{x} \ \ \sum_{t=0}^{n}\sum_{k\in S}\|z_t^k -h_t^k(X))\|^2_{\Omega_t^k}. \end{aligned} X∗=argmaxx t=0∏nk∈S∏exp(−21∥ztk−htk(X))∥Ωtk2)=argminx t=0∑nk∈S∑∥ztk−htk(X))∥Ωtk2. - 注意这里的马氏距离要进行一个分解才可以进行最小二乘
∥ r ∥ Ω 2 = r T Ω − 1 r \|r\|^2_Ω = r^T\Omega^{-1}r ∥r∥Ω2=rTΩ−1r
GPS残差计算
- P t G P S = [ x t w , y t w , z t w ] T P_t^{GPS} = [x_t^w,y_t^w,z_t^w]^{T} PtGPS=[xtw,ytw,ztw]T
- z t G P S z_t^{GPS} ztGPS是GPS的测量值
- h t G P S ( X ) h_t^{GPS}(X) htGPS(X)是GPS的观测方程
- z t G P S − h t G P S ( X ) = z t G P S − h t G P S ( x t ) = P t G P S − P t w . \begin{aligned} z_t^{GPS}-h_t^{GPS}(X) &= z_t^{GPS}-h_t^{GPS}(x_t)\\ &=P_t^{GPS}-P_t^w. \end{aligned} ztGPS−htGPS(X)=ztGPS−htGPS(xt)=PtGPS−Ptw.
- x i = x i − 1 + d x x_i = x_{i-1}+dx xi=xi−1+dx
- d x dx dx通过VIO求得
- x x x可以通过上一时刻状态以及当前时刻与上一时刻的位姿变换计算出来。具体到本文的方法，就是利用VO得到当前时刻和上一时刻的相对位姿 d x dx dx，加到上一时刻的位姿上 x i − 1 x_{i-1} xi−1，得到当前时刻的位姿 x i x_i xi
- 注意: gps信号接受的时候会伴随一个accuracy,可以理解为gps信号的协方差,这个协方差的大小取决于gps信号接受时卫星的数量,卫星的数量越多,这个协方差就会越小
代码段
- 对gps信号数据的处理部分
- 所有的gps信号数据存于gpsQueue中,t是VO的时间戳
- t < 0.01 s t<0.01s t<0.01s说明是VO的10 m s ms ms之前的数据,这部分数据直接舍弃掉
- t > 0.01 s t>0.01s t>0.01s说明VO的数据滞后了,此时应该等待VO数据传来
- t − 0.01 s < t < t + 0.01 s t-0.01s
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
if (gps_t >= t - 0.01 && gps_t <= t + 0.01)
{
...
printf("receive covariance %lf \n", pos_accuracy);
globalEstimator.inputGPS(t, latitude, longitude, altitude, pos_accuracy);
gpsQueue.pop();
break;
}
else if (gps_t < t - 0.01)
gpsQueue.pop();
else if (gps_t > t + 0.01)
break;
}
- 对gps信号转换坐标系
- 这里是输入的VO数据的10ms内的GPS数据进行坐标系转换,GPS2XYZ函数将GPS的经纬度坐标转换为ENU坐标,并且第一帧的GPS数据作为原点 [ 0 , 0 , 0 ] [0,0,0] [0,0,0]
- 转换之后的gps数据和协方差一起存入到GPSPositionMap中
void GlobalOptimization::inputGPS(double t, double latitude, double longitude, double altitude, double posAccuracy)
{
double xyz[3];
GPS2XYZ(latitude, longitude, altitude, xyz);
vector<double> tmp{xyz[0], xyz[1], xyz[2], posAccuracy};
//printf("new gps: t: %f x: %f y: %f z:%f \n", t, tmp[0], tmp[1], tmp[2]);
GPSPositionMap[t] = tmp;
newGPS = true;
}
- 对VIO数据的处理
- 这里输入的就是VO的pose, [ x , y , z , w ] [x,y,z,w] [x,y,z,w]
- 这些pose是在VO坐标系下的,来了就先存到localPoseMap中
- 这些pose会转换到gps下,也就是local转换到global的观测方程,转换方式用简单的表达就是
T G P S = T G P S − > V O ∗ T V O T_{GPS}=T_{GPS->VO} * T_{VO} TGPS=TGPS−>VO∗TVO - 所以这里比较关键的一个参数就是GPS坐标系和VO坐标系之间的外参 T G P S − > V O T_{GPS->VO} TGPS−>VO, 也就是代码中的WGPS_T_WVIO矩阵, 这个矩阵也是后面使用ceres进行优化后进行计算的一个重要参数,计算方式为
T G P S − > V O = T b o d y − > G P S − 1 ∗ T b o d y − > V O T_{GPS->VO}=T_{body->GPS}^{-1}*T_{body->VO} TGPS−>VO=Tbody−>GPS−1∗Tbody−>VO
void GlobalOptimization::inputOdom(double t, Eigen::Vector3d OdomP, Eigen::Quaterniond OdomQ)
{
mPoseMap.lock();
vector<double> localPose{OdomP.x(), OdomP.y(), OdomP.z(),
OdomQ.w(), OdomQ.x(), OdomQ.y(), OdomQ.z()};
localPoseMap[t] = localPose;
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;
lastP = globalP;
lastQ = globalQ;
...
mPoseMap.unlock();
}
- 优化部分
void GlobalOptimization::optimize() {
while (true) {
if (newGPS) {
newGPS = false;
...
//...ceres
ceres::LocalParameterization *local_parameterization =
new ceres::QuaternionParameterization();
// add param
mPoseMap.lock();
int length = localPoseMap.size();
// w^t_i w^q_i
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++) {
...
//初始添加的是gps坐标系下的参数
problem.AddParameterBlock(q_array[i], 4, local_parameterization);
problem.AddParameterBlock(t_array[i], 3);
}
...
for (iterVIO = localPoseMap.begin(); iterVIO != localPoseMap.end();
iterVIO++, i++) {
// vio factor
iterVIONext = iterVIO;
iterVIONext++;
if (iterVIONext != localPoseMap.end()) {
...
//添加i帧和j帧vio(VIO坐标系下)
wTi.block<3, 3>(0, 0) =...
wTi.block<3, 1>(0, 3) =...
wTj.block<3, 3>(0, 0) =...
wTj.block<3, 1>(0, 3) =...
// 第j帧到第i帧的变换矩阵
Eigen::Matrix4d iTj = wTi.inverse() * wTj;
...
iQj = iTj.block<3, 3>(0, 0);
iPj = iTj.block<3, 1>(0, 3);
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]);
}
// gps factor
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]);
// printf("inverse weight %f \n", iterGPS->second[3]);
problem.AddResidualBlock(gps_function, loss_function, t_array[i]);
}
}
...
//求解最小二乘
iter = globalPoseMap.begin();
for (int i = 0; i < length; i++, iter++) {
//取出优化参数
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();
double t = iter->first;
//VIO到body坐标系的q
WVIO_T_body.block<3, 3>(0, 0) =...localPose_q
//VIO到body坐标系的t
WVIO_T_body.block<3, 1>(0, 3) =...localPose_t
//GPS到body坐标系的q
WGPS_T_body.block<3, 3>(0, 0) =...globalPose_q
//GPS到body坐标系的t
WGPS_T_body.block<3, 1>(0, 3) =...globalPose_t
//GPS到VIO坐标系的变换矩阵
WGPS_T_WVIO = WGPS_T_body * WVIO_T_body.inverse();
}
}
//更新全局路径
updateGlobalPath();
mPoseMap.unlock();
}
...
}
return;
}
參考文献
A General Optimization-based Framework for Global Pose Estimation with Multiple Sensors
VINS Fusion算法解读