(三)gmapping详细代码解析
A、里程计运动模型
我们先来看一下Gmapping里面实现的里程计模型代码:
/*
@desc 里程计运动模型
@p 表示粒子估计的最优位置(机器人上一个时刻的最优位置)
@pnew 表示里程计算出来的新的位置
@pold 表示里程计算出来的旧的位置(即上一个里程计的位置)
*/
OrientedPoint
MotionModel::drawFromMotion(const OrientedPoint& p, const OrientedPoint& pnew, const OrientedPoint& pold) const{
double sxy=0.3*srr;
OrientedPoint delta=absoluteDifference(pnew, pold); // 计算新位姿相对于旧位姿的偏移量
/*初始化一个点*/
OrientedPoint noisypoint(delta);
/*走过的X、y、z轴方向的距离加入噪声*/
noisypoint.x+=sampleGaussian(srr*fabs(delta.x)+str*fabs(delta.theta)+sxy*fabs(delta.y));
noisypoint.y+=sampleGaussian(srr*fabs(delta.y)+str*fabs(delta.theta)+sxy*fabs(delta.x));
noisypoint.theta+=sampleGaussian(stt*fabs(delta.theta)+srt*sqrt(delta.x*delta.x+delta.y*delta.y));
/*限制角度的范围*/
noisypoint.theta=fmod(noisypoint.theta, 2*M_PI);
if (noisypoint.theta>M_PI)
noisypoint.theta-=2*M_PI;
/*把加入了噪声的值 加到粒子估计的最优的位置上 即得到新的位置(根据运动模型推算出来的位置)*/
return absoluteSum(p,noisypoint);
}
-
OrientPoint点的结构:
/*带方向的点 即除了位置之外,还有角度,可以理解为机器人的位姿*/
template <class T, class A>
struct orientedpoint: public point<T>
{
inline orientedpoint() : point<T>(0,0), theta(0) {};
inline orientedpoint(const point<T>& p);
inline orientedpoint(T x, T y, A _theta): point<T>(x,y), theta(_theta){}
inline void normalize();
inline orientedpoint<T,A> rotate(A alpha)
{
T s=sin(alpha), c=cos(alpha);
A a=alpha+theta;
a=atan2(sin(a),cos(a));
return orientedpoint(
c*this->x-s*this->y,
s*this->x+c*this->y,
a);
}
A theta;
};
-
absoluteDifference函数:
absoluteDifference()这个函数是将t时刻与t-1时刻里程计测量数据之差转换到t-1时刻的车辆坐标系下,计算位姿差
/*
@desc 两个位姿的差值,算出来P1在以P2为原点的坐标系里面的坐标。
*/
template <class T, class A>
orientedpoint<T,A> absoluteDifference(const orientedpoint<T,A>& p1,const orientedpoint<T,A>& p2)
{
orientedpoint<T,A> delta=p1-p2;
delta.theta=atan2(sin(delta.theta), cos(delta.theta));
double s=sin(p2.theta), c=cos(p2.theta);
return orientedpoint<T,A>(c*delta.x+s*delta.y,
-s*delta.x+c*delta.y, delta.theta);
}
- sampleGaussian函数(这里我把源代码中的注释给删掉了):
double sampleGaussian(double sigma, unsigned int S) {
if (S!=0)
{
srand(S);
}
if (sigma==0)
return 0;
return pf_ran_gaussian (sigma);
}
sampleGaussian函数里面的pf_ran_gaussian()函数,从方差为sigma,均值为0的高斯分布中抽取随机数,
参考:https://www.taygeta.com/random/gaussian.html(没太懂):
double pf_ran_gaussian(double sigma)
{
double x1, x2, w;
double r;
do
{
do { r = drand48(); } while (r == 0.0);
x1 = 2.0 * r - 1.0;
do { r = drand48(); } while (r == 0.0);
x2 = 2.0 * drand48() - 1.0;
w = x1*x1 + x2*x2;
} while(w > 1.0 || w==0.0);
return(sigma * x2 * sqrt(-2.0*log(w)/w));
}
- fod(x,y)是返回x/y的余数,将角度限制到(-pi,pi)之间
gmapping的里程计采样模型采取的方法是《概率机器人》里的里程计运动模型,具体如下:
- 运动模型示意图:
伪代码:
(1)2-4行表示将t时刻数据转换到t-1时刻的车辆坐标系下
(2)5-6行给新的偏移量加入一个包含噪声的偏移值,噪声是一个高斯采样值
(3)8-9行把得到的新的偏移量转换到没有噪声的车辆坐标下,再叠加t-1时刻的位姿,得到t时刻的位姿估计
控制量ut就是里程计给出的信息,,其中
是里程计信息在t-1时刻的位姿,是里程计在t时刻位姿,因为里程计是机器人内部的传感器,它计算的是机器人一点都没噪声的理想状态时的行进距离,所以由于机器人在行进的时候的偏移和打滑,这个
是不准确的,所以在5~6行我们加入一定的噪声来表示机器人实际行进的“真”值。
B、scan-match模型
先上代码,gmapping里的函数scanMatch(),函数位于gridslamprocessor.hxx中;
来看在scanMatch中调用第一个比较重要的函数---optimize函数,位于scanmatcher.cpp中:
double ScanMatcher::optimize(OrientedPoint& pnew, const ScanMatcherMap& map, const OrientedPoint& init, const double* readings) const
{
double bestScore=-1;
/*计算当前位置的得分*/
OrientedPoint currentPose=init;
double currentScore=score(map, currentPose, readings);
/*所有时的步进增量*/
double adelta=m_optAngularDelta, ldelta=m_optLinearDelta;
/*精确搜索的次数*/
unsigned int refinement=0;
/*搜索的方向*/
enum Move{Front, Back, Left, Right, TurnLeft, TurnRight, Done};
//enum Move{Front, Back, Left, Right, TurnLeft, TurnRight, Done};
int c_iterations=0;
do
{
/*如果这一次(currentScore)算出来比上一次(bestScore)差,则有可能是走太多了,要减少搜索步长 这个策略跟LM有点像*/
if (