文章目录
1.前言2.场景介绍3.最小二乘理论推导3.1方程建立3.2计算雅可比方程
4.代码5.结果与结论
1.前言
本人一直致力于定位研究，接触各种定位算法，最终需要输出比较完美的定位轨迹。一直以来滤波算法被运用于轨迹融合，但是研究表明，基于最小二乘的非线性优化方案，有更好的表现。本文基于谷歌开发的Ceres Solver非线性最小二乘优化包，进行轨迹优化。
2.场景介绍
为了演示的方便，我们抛弃了高度，做了一个二维轨迹仿真。在一条10米的路上，一个机器人从（-5，0，0）出发，沿着（1，0，0）以0.1m/s的速度走向（5，0，0），我们能得到的数据是每秒的速度和位置，但是都是有误差的。
3.最小二乘理论推导
3.1方程建立
这时就需要建立最小二乘误差方程。如下：
min
P
i
,
P
j
∑
i
N
−
1
∣
∣
V
i
∗
Δ
t
−
(
P
i
−
P
j
)
∣
∣
{\min_{P_i,P_j} \sum_i^{N-1}||V_i*\Delta t-(P_i-P_{j})||}
Pi,Pjmini∑N−1∣∣Vi∗Δt−(Pi−Pj)∣∣ 其中
Δ
t
{\Delta t}
Δt为
P
i
{P_i}
Pi到
P
j
{P_j}
Pj的时间。
P
i
{P_i}
Pi为
i
{i}
i时刻位置，
P
j
{P_j}
Pj为
j
{j}
j时刻的位置。
3.2计算雅可比方程
上式也是残差方程：
r
e
s
i
d
u
a
l
=
V
i
∗
Δ
t
−
(
P
i
−
P
j
)
{residual=V_i*\Delta t-(P_i-P_{j})}
residual=Vi∗Δt−(Pi−Pj) 残差的维度为三维。 由于我们将位置当作优化参数，所以参数有两个，既有两参数块，一个为
P
i
{P_i}
Pi,一个为
P
j
{P_j}
Pj。 所以有
j
a
c
o
b
i
a
n
s
[
0
]
=
∂
r
e
s
i
d
u
a
l
∂
P
i
=
[
−
1
0
0
0
−
1
0
0
0
−
1
]
{jacobians[0]=\frac{\partial residual}{\partial P_i}=\begin{bmatrix} -1 & 0& 0 \\ 0 & -1 &0 \\ 0 &0 & -1 \end{bmatrix}}
jacobians[0]=∂Pi∂residual=⎣⎡−1000−1000−1⎦⎤ 维度为：
3
∗
3
{3*3}
3∗3
j
a
c
o
b
i
a
n
s
[
1
]
=
∂
r
e
s
i
d
u
a
l
∂
P
j
=
[
1
0
0
0
1
0
0
0
1
]
{jacobians[1]=\frac{\partial residual}{\partial P_j}=\begin{bmatrix} 1 & 0& 0 \\ 0 & 1 &0 \\ 0 &0 & 1 \end{bmatrix}}
jacobians[1]=∂Pj∂residual=⎣⎡100010001⎦⎤ 维度为：
3
∗
3
{3*3}
3∗3
4.代码
#include <iostream>
#include <opencv2/core/core.hpp>
#include <ceres/ceres.h>
#include <chrono>
#include <eigen3/Eigen/Dense>
#include "ros/ros.h"
#include <geometry_msgs/Vector3Stamped.h>
#include<nav_msgs/Path.h>
using namespace std;
#define C_PI (double)3.141592653589793
nav_msgs::Path path;
nav_msgs::Path path1;
ros::Publisher path_pub;
ros::Publisher path_pub_n;
int N=101;
// 代价函数的计算模型
class DeltaDis:public ceres::SizedCostFunction<3,3,3>
{
public:
DeltaDis()=delete;
DeltaDis(Eigen::Vector3d deltadis)
{
Deltadis=deltadis;
}
virtual ~DeltaDis(){}
virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const
{
Eigen::Vector3d Pi(parameters[0][0], parameters[0][1], parameters[0][2]);
Eigen::Vector3d Pj(parameters[1][0], parameters[1][1], parameters[1][2]);
Eigen::Map<Eigen::Matrix<double, 3, 1>> residual(residuals);
residual=Deltadis-(Pi-Pj);
// cout<<"residual:"<<residual;
if(jacobians)
{
if (jacobians[0])
{
Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::RowMajor>> jacobian_pose_i(jacobians[0]);
jacobian_pose_i.setZero();
jacobian_pose_i(0,0)=-1.0;
jacobian_pose_i(2,2)=-1;
jacobian_pose_i(1,1)=-1;
}
if (jacobians[1])
{
Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::RowMajor>> jacobian_pose_j(jacobians[1]);
jacobian_pose_j.setZero();
jacobian_pose_j(0,0)=1;
jacobian_pose_j(2,2)=1;
jacobian_pose_j(1,1)=1;
}
}
}
private:
Eigen::Vector3d Deltadis;
};
// Returns true if the solve was successful.
bool SolveOptimizationProblem(ceres::Problem* problem) {
CHECK(problem != NULL);
ceres::Solver::Options options;
options.max_num_iterations = 100;
options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
ceres::Solver::Summary summary;
ceres::Solve(options, problem, &summary);
std::cout << summary.FullReport() << '\n';
return summary.IsSolutionUsable();
}
enum SIZE_PARAMETERIZATION
{
SIZE_POSE = 3,
};
int main ( int argc, char** argv )
{
ros::init(argc, argv, "mp_node");
ros::NodeHandle n;
path_pub=n.advertise<nav_msgs::Path>("trajectory1",1,true);
path_pub_n=n.advertise<nav_msgs::Path>("trajectory2",1,true);
double Pi[12][SIZE_POSE];
double Pj[12][SIZE_POSE];
path.header.frame_id="map";
path1.header.frame_id="map";
Eigen::Vector3d Pose[N];
Eigen::Vector3d T;
T.setZero();
double p_sigma=0.1; // 噪声Sigma值
cv::RNG rng;
Eigen::Vector3d Vs[N]; // OpenCV随机数产生器
for(int i=0;i<N;i++)
{
path.header.stamp=ros::Time::now();
// T((i/25)%2)=T((i/25)%2)+pow(-1,i/50)*1;
T.setIdentity();
T=T/10;
double a=rng.gaussian (p_sigma);
Vs[i]=T+Eigen::Vector3d(a/10,a/10,0);;
Pose[i]=T*i;
Pose[i](0)=Pose[i](0)-5;
Pose[i]=Pose[i]+Eigen::Vector3d(a*2,a*5-a*a,0);
geometry_msgs::PoseStamped thie_Pose;
thie_Pose.pose.position.x=Pose[i](0);
thie_Pose.pose.position.y=Pose[i](1);
thie_Pose.pose.position.z=Pose[i](2);
thie_Pose.pose.orientation.x=1;
thie_Pose.pose.orientation.y=1;
thie_Pose.pose.orientation.z=1;
thie_Pose.pose.orientation.w=1;
thie_Pose.header.stamp=ros::Time::now();
thie_Pose.header.frame_id="path";
path.poses.push_back(thie_Pose);
path_pub.publish(path);
}
double pose_s[N][3];
ceres::Problem problem;
for(int i=1;i<N;i++)
{
pose_s[i][0]=Pose[i](0);
pose_s[i][1]=Pose[i](1);
pose_s[i][2]=Pose[i](2);
}
for(int i=1;i<N;i++)
{
Eigen::Vector3d deldis=Vs[i-1];
problem.AddParameterBlock(pose_s[i-1],SIZE_POSE);
problem.AddParameterBlock(pose_s[i],SIZE_POSE);
DeltaDis* deldisf=new DeltaDis(deldis);
problem.AddResidualBlock(deldisf, nullptr, pose_s[i-1],pose_s[i]);
}
SolveOptimizationProblem(&problem);
for(int i=0;i<N;i++)
{
geometry_msgs::PoseStamped this_Pose;
this_Pose.pose.position.x=pose_s[i][0];
this_Pose.pose.position.y=pose_s[i][1];
this_Pose.pose.position.z=pose_s[i][2];
this_Pose.pose.orientation.x=1;
this_Pose.pose.orientation.y=1;
this_Pose.pose.orientation.z=1;
this_Pose.pose.orientation.w=1;
this_Pose.header.stamp=ros::Time::now();
this_Pose.header.frame_id="n_path";
path1.poses.push_back(this_Pose);
path_pub_n.publish(path1);
}
int a;
cin>>a;
return 0;
}
5.结果与结论
绿色的为仿真轨迹，红色的为优化轨迹。可以看到优化结果相当的好。
