系列文章目录

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

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

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

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

文章目录

系列文章

文章目录

前言

一、梯度下降法（GD）

二、最速下降法（SD）

总结

前言

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

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

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

$\underset{x}{min}\sum \left \| y_{i}-f(x_{i}) \right \|^{2}_{\sum _{i}^{-1}}$

其中 $f(x_{i})$ 是非线性函数， $\sum{_{i}^{-1}}$ 表示协方差矩阵

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

$\psi (x)=\sum \left \| y_{i}-f(x_{i}) \right \|^{2}_{\sum^{-1}_{i}}$

一、梯度下降法（GD）

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

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

算法具体表示如下：

$do\{ x^{(k+1)} = x^{(k)} - \alpha \cdot \bigtriangledown \psi (x^{(k)}) \}while(\psi (x^{(k+1)}) < \psi(x^{(k)}))$

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

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

在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;
  end
  n = n+1;
  point(n,:) = [x y b]; 
end

实验结果：

二、最速下降法（SD）

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

$\alpha _{k}=\underset{\alpha \geq 0}{argmin} \psi(x^{(k)}-\alpha \cdot \bigtriangledown \psi (x^{(k)}) )$

如下图所示：

自变量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;
  end
  n = n+1;
  point(n,:) = [x y b]; 
end

实验结果：