系列文章目录

·【算法系列】卡尔曼滤波算法

·【算法系列】非线性最小二乘求解-直接求解法

·【算法系列】非线性最小二乘求解-梯度下降法

·【算法系列】非线性最小二乘-高斯牛顿法 

文章目录

系列文章

文章目录

前言

一、梯度下降法（GD）

二、最速下降法（SD）

总结

前言

SLAM问题常规的解决思路有两种，一种是基于滤波器的状态估计，围绕着卡尔曼滤波展开；另一种则是基于图论（因子图）的状态估计，将SLAM问题建模为最小二乘问题，而且一般是非线性最小二乘估计去求解。

非线性最小二乘有多种解法，本篇博客介绍梯度下降法系列求解最小二乘问题。

非线性最小二乘的一般形式如下：

其中​是非线性函数，​表示协方差矩阵

为了阐述方便，进行如下表示：

一、梯度下降法（GD）

梯度下降法是使自变量x按一定步长沿梯度的反方向进行调整，对应的函数值就会下降，这样不断调整x，直到函数取值下降到最小为止，以下图进行具体说明。

这里的x是一维变量，梯度可以理解为一阶导数，初值选在x1的位置，此时一阶导数值为负，梯度的反方向为正，所以应该增加x的值，按照步长调整至x2，依次迭代；当到达x4位置时，一阶导数变为正值，梯度反方向为负，应该减小x的值，反复迭代，假设收敛到了一个最小值x5，算法结束。

算法具体表示如下：

梯度下降法的原理和实现都很简单，但它的缺点也很明显：

1. 对初值敏感。在图中很容易发现，收敛获得的最小值，只是算法以为的最小值，是个局部最小值，而真实的最小值在橙点处，这跟初值的选取有关。
2. 步长的选择至关重要。如果步长太小，收敛速度很慢，需要迭代很多次才能到的目标点；如果步长太大，很可能错过目标点，形成在最小值附件来回震荡的情况。

在SLAM中，状态由三维坐标和空间姿态角两部分组成，空间姿态角一般用四元数表示，由于存在内部额外约束，无法进行求导和加法迭代运算，这时就要装换到李代数上进行求导和求和运算。

MATLAB实验：

主函数：

% 目标函数为 z=f(x,y)=(x^2+y^2)/2
clear all;
clc;
%构造函数
fun = inline('(x^2+y^2)/2','x','y');
dfunx = inline('x','x','y');
dfuny = inline('y','x','y'); x0 = 2;y0 = 2;                                  %初值
p = 0.1;                                        %步长[x,y,n,point] = GD(fun,dfunx,dfuny,x0,y0,p);    %梯度下降法
%[x,y,n,point] = SD(fun,dfunx,dfuny,x0,y0);    %最速下降法figure
x = -1:0.1:2;
y = x;
[x,y] = meshgrid(x,y);
z = (x.^2+y.^2)/2;
surf(x,y,z)    %绘制三维表面图形
% hold on
% plot3(point(:,1),point(:,2),point(:,3),'linewidth',1,'color','black')
hold on
scatter3(point(:,1),point(:,2),point(:,3),'r','*');

GD函数：

%% 梯度下降法
function [x,y,n,point] = GD(fun,dfunx,dfuny,x,y,p)
%输入：fun:函数形式 dfunx(y):梯度（导数） x(y):初值 p:步长
%输出：x(y):计算出的自变量取值 n:迭代次数 point:迭代点集%初始化
a = feval(fun,x,y);
n=1;
point(n,:) = [x y a];
while (1) a = feval(fun,x,y);           %当前时刻值x = x - p*(feval(dfunx,x,y)); %下一时刻自变量y = y - p*(feval(dfuny,x,y)); %下一时刻自变量b = feval(fun,x,y);           %下一时刻值 if(b>=a)break;endn = n+1;point(n,:) = [x y b]; 
end

实验结果：

二、最速下降法（SD）

最速下降法解决的是梯度下降法中关于步长选取的问题，最速下降法中每次迭代都会找到一个合适的步长，使得函数沿当前梯度反方向下降，用数学语言描述如下：

如下图所示：

自变量x是二维向量，此时的梯度方向与等高线切线方向垂直，每次都会选取一个合适的步长，使得取值越来越趋近于最小值，每次的步长都不是固定值，保证了函数取值一直是下降的。

MATLAB实验：

主函数：

% 目标函数为 z=f(x,y)=(x^2+y^2)/2
clear all;
clc;
%构造函数
fun = inline('(x^2+y^2)/2','x','y');
dfunx = inline('x','x','y');
dfuny = inline('y','x','y'); x0 = 2;y0 = 2;                                  %初值
p = 0.1;                                        %步长%[x,y,n,point] = GD(fun,dfunx,dfuny,x0,y0,p);    %梯度下降法
[x,y,n,point] = SD(fun,dfunx,dfuny,x0,y0);    %最速下降法figure
x = -1:0.1:2;
y = x;
[x,y] = meshgrid(x,y);
z = (x.^2+y.^2)/2;
surf(x,y,z)    %绘制三维表面图形
% hold on
% plot3(point(:,1),point(:,2),point(:,3),'linewidth',1,'color','black')
hold on
scatter3(point(:,1),point(:,2),point(:,3),'r','*');

SD函数：

%% 梯度下降法
function [x,y,n,point] = SD(fun,dfunx,dfuny,x,y)
%输入：fun:函数形式 dfunx(y):梯度（导数） x(y):初值
%输出：x(y):计算出的自变量取值 n:迭代次数 point:迭代点集%初始化
a = feval(fun,x,y);
n=1;
point(n,:) = [x y a];
p=0.01:0.01:0.1;       %步长范围while (1) [m,i]=min(x - p*(feval(dfunx,x,y)));  %求解合适的步长a = feval(fun,x,y);                   %当前时刻值x = x - p(i)*(feval(dfunx,x,y));      %下一时刻自变量y = y - p(i)*(feval(dfuny,x,y));      %下一时刻自变量b = feval(fun,x,y);                   %下一时刻值if(b>=a)break;endn = n+1;point(n,:) = [x y b]; 
end

 实验结果：

总结

虽然最速下降法解决了步长选取的问题，但是在实际使用中，不可避免的会出现初值选取不合适导致获得局部最小值的问题，接下来将介绍高斯-牛顿的方法、裂纹伯格-马夸尔的方法及其变种。

实际应用中应对这几种方法灵活选择。