1. 最小二乘法

最小二乘法是一种最基本的数学工具其在机器人系统中有着诸多的应用，比如在IMU标定中我们会使用最小二乘法来拟合IMU待标定的参数，在力分配环节中用它来求取需要的足底力，当然其和卡尔曼滤波等算法也有着非常重要的联系。目前在网络上有许多最小二乘法的相关资料，无论是在代码和原理上都有着丰富的资料可以查阅，但总体上还是无法提供一个能快速理解的脉络，特别是涉及到实际应用问题中如椭球拟合磁力计标定等大家都直接给出许多相关例子，但在一些细节部分进行跳过导致难以理解，比如同样对于拟合一个曲线的最小二乘问题有的资料中给出了直接用测量数据组成矩阵然后求逆，而有的则采用求伪逆（最小二乘解），而有的又采用求偏导的当时，因此下面给出一个以我个人能较好理解最小二乘法的思路并且以更白话的方式进行阐述。

（1）最小二乘描述的问题

最小二乘法通常意义下希望能解决的问题等效于求解一个系统模型中最优未知系数，该系数能尽量最小化测量与模型的误差，如许多资料中常使用的拟合直线例子，其等式或模型如下：

其中k和b为带优化的系数，x和y为系统状态与测量值，则通过给定一组数（由于有两个未知变量，因此最少需要2组测量值）来求取系数，则当测量值与未知系数一样时就可以构建对应的线性方程组：

上式可以通过消元的方式求解，则当有N个散点时其变成拟合直线问题，即求取系数能使得所有点到该直线的垂直距离平方和最小，即定义误差：

求系数使得下式最小化：

则如果还是采用之前的线性方程组描述形式为:

可见采用矩阵的方式表达其将转换为在优化问题中常见的多元线性回归形式，其设定优化函数为：

来求解未知系数，该思想可以扩展到目前的MPC等基于非线性优化手段的最优控制理论中。

当前我们需要注意到上式中的矩阵A如果其是方阵的话刚好为可逆矩阵，此时可直接通过求逆的方法获取未知参数矩阵，而当有N个测量值即构成了超定方程组需求取A的伪逆即：

则未知系数为：

（2）最小二乘法拟合曲线的多种形式-“直接法”

在上面我们给出了常见直线拟合的最小二乘问题，注意该问题是一个线性的最小二乘问题，因为所有的系数k和b均没有非线性形式，如：

另外也没有出现两个未知系数相乘的非线性形式，因此总体来说较为简单，这也是为什么仅参考网上的例子让许多人无法用其解决实际问题的原因，因为在许多实际系统和问题中往往会出现多个系数相乘或他们高阶非线性项的形式，另外网上例子往往仅给出二元线性回归（即给定x和y）的问题，而当期上升到三个变量时如不理解最小二乘原理是会不知道如何下手。在IMU磁力计标定中遇到的椭球拟合就需要解决这样一个问题，那首先我们就先给定如下一个典型的三维曲线待拟合方程或模型：

可以看到上面这个模型总共有6个未知系数（最少需要6组测量值），且均为线性形式总共有三个系统变量，因此首先可以采用最传统的方程组形式来描述求解，即为“直接法”如：

同理我们不必考虑有多少个系统变量，首先将不包含未知系数的部分都放置到等式右边比写成Ax=b的矩阵形式：

则进一步考虑上式对应的A矩阵行数，如与未知系数数量相等构成方阵且可逆，则可直接求取逆矩阵，而如果数量大于5则构成超定方程则求取其伪逆与b矩阵相乘求取出未知系数即可（在matlab中即使用pinv求取伪逆）。

（2）最小二乘法拟合曲线的多种形式-“间接法”

直接法中我们直接通过观测值构建Ax=b的线性方程组矩阵形式通过求逆运算可以快速求取对应系数，而对于测量数据多于系数数量的拟合问题则通过直接使用最小二乘法已经推导好的结果即求取A伪逆的运算也能得到对应解，但该方法且不说需要保证矩阵A的列满秩要求，在实际应用中我们很难保证遇到像其使用拟合曲线中对应系数均为线性表达，而如典型使用最小二乘法拟合圆的例子中我们会遇到系数有高阶项或者多个系数相乘的形式，那此时应该如果处理？

如上式中存在着非线性系数部分，无法采用直接法将其作为待求矩阵向量x，因此间接法所采用的技巧即数学中最常见的变量替换法，通过定义新的变量统一表示非线性系数，在采用直接法求取后进一步基于其存在的内在约束或待定系数的方式进行求解，以上式为例我们就可以定义如下系数变量和新的模型：

同理采用直接法将不包含未知系数的部分移到等式右边，由于新模型均为线性系数形式，因此通过采用直接法结合观测数据拟合得到最终的系数A,B,C，进一步通过对应原始模型可以得到：

而其中具体r系数的符号可结合模型本身约束，如r代表圆的半径为正号。

（3）最小二乘法拟合曲线的多种形式-“广义法”

综上，间接法可以说是本质上还是直接法只不过使用中间变量换元来消除一些简单的非线性部分如某系数二阶量或其倍数等，但对于多个系数相乘或者更高阶的系数来说该方法可能最终无法求取出唯一系数，因此对于这些非线性最小二乘问题需要采用其他优化方法如LM法、高斯牛顿或梯度下降的方法来求取，这里对这些问题先不赘述我们首先仅解决一个计算量上的问题。

在上面两种方法中可以看到矩阵A是否求伪逆是依靠测量数据量与未知系数的关系来确定的，而在拟合问题中给定的数据量越大最终拟合系数精度越高这也意味着矩阵A的行数会越来越大，而求取伪逆的运算量也会成倍增大。为规避这个问题我们需要采用一种更准确的最小化形式来构建最小二乘问题，我们回到之前提到的目标函数：

可以看到广义上的最小二乘问题实际上构建的是最小化问题而不是直接法中使用的多元线性方程组形式，其目的是找到一组最优系数来最小化测量值与模型值误差，而由于误差中包含系数因此我们可以通过求取F对应系数的偏导值为0即误差函数的极点来求解上述问题，即我们实际构建的目的是构建如下的线性方程组而不是直接使用测量值所建立的线性方程组：

则仍然以间接法中给出的二位平面圆拟合问题为例，假设我们有N组散点数据，首先使用间接法换元消除其中的非线性部分：

进一步构建第i组测量数据误差方程（将所有变量移动到等式左边）如下注意这里谁减谁并不重要因为后续采用的是最小化平方和的代价函数：

则所构建的最小二乘问题为：

对F求取各变量的偏导有：

则同理将不包含系数的部分移动到等式右边有：

写成矩阵的形式有：

可以看到上式又重新构成了Ax=b的齐次方程组形式，而与直接法中不同的是这里的A矩阵维度不会随观测数据量N的变大而增大，即其计算量是固定的并且矩阵A也一定是方阵，在保证一定采样数量让其列满秩的前提下仅通过求逆就可以计算出待求系数，计算复杂度相比直接法大大下降，而如果在代价函数中直接引入1/2可以消除最矩乘2的冗余计算，如果引入1/N则矩阵A和b中相关变量计算就变成了求取一段时间内测量数据平均值的问题，进一步在嵌入式编程中我们可以将其转换为一种增量采集的形式，如磁力计标定或在线标定模型系数时会变得更加方便。

（4）最小二乘法拟合问题的总结

上文给出了以我个人理解的线性最小二乘法以及实际应用的思路，其大概流程归纳如下，首先列出需要拟合的方程或模型，确认有没有非线性部分，如可以用变量代替则使用间接法换元，如果新模型系数数量较少且测量数量较少可直接使用直接法求逆计算，如数据量大或需要实时更新则采用广义法求解。

可见目前网上相关资料之所以有点混乱的原因是没有整理清楚最小二乘法使用的流程，许多例子中直接法、间接法和广义法交替使用或者一笔带过，导致最终的公式罗列复杂难以理解。对于最小二乘法的使用来说最主要的工作就是将其转化为线性形式否则只能使用其他方法来求解，另外就是依据观测数据量与实际未知系数确定是否直接求逆，而通常情况下广义方法使用起来可能更加合理，但在许多仅有三到六个测量数据的拟合问题中，直接使用测量值构建多元线性方程组求取最小二乘解的方案可能更加常见，但对于一些需要连续长期采集较多数据的拟合或标定问题来说广义法更具优势。

下面以拟合一个3阶曲线为例给出MATLAB仿真代码：

close all;
clear all;
clc;
%%拟合3阶曲线
%y=a*x^3+b*x^2+c*x+d
%制造测量
N=50;
Dn=N/2;
d_sampe=5/N;
w=0.03;
a=-0.5;
b=1.0;
c=-0.3;
d=0.03;
p_real=[a;b;c;d]
for i=1:N
x(i) = i/Dn;
y_real(i)=a*x(i)^3+b*x(i)^2+c*x(i)+d;
y_measure(i)=a*x(i)^3+b*x(i)^2+c*x(i)+d+randn(1)*w;
end

%%直接法
A(1,:)=[x(1)^3,x(1)^2,x(1),1];
b(1,:)=[y_measure(1)];
for i=2:N
A=[A(:,:);
   x(i)^3,x(i)^2,x(i),1];
b=[b(:,:);
   y_measure(i)];
end
p_d=pinv(A)*b

for i=1:N
x(i) = i/Dn;
y_est(i)=p_d(1)*x(i)^3+p_d(2)*x(i)^2+p_d(3)*x(i)+p_d(4);
end

figure(1)
plot(x,y_real,'-.r');
hold on;
plot(x,y_measure,'sb');
hold on;
plot(x,y_est,'-k');
grid on;

%%广义法
%F=min 0.5/N*(a*x^3+b*x^2+c*x+d-y)^2
%Fda= 1/N*sum (a*x^3+b*x^2+c*x+d-y)*x^3 =0 
%-> 1/N*sum (a*x^6+b*x^5+c*x^4+d*x^3-y*x^3) =0
%-> 1/N*sum (a*x^6+b*x^5+c*x^4+d*x^3) =1/N*sum (y*x^3)
sum11=0;
sum12=0;
sum13=0;
sum14=0;
sumy1=0;
for i=1:N
x(i) = i/Dn;
sum11=sum11+x(i)^6;
sum12=sum12+x(i)^5;
sum13=sum13+x(i)^4;
sum14=sum14+x(i)^3;
sumy1=sumy1+y_measure(i)*x(i)^3;
end

%F=min 0.5/N*(a*x^3+b*x^2+c*x+d-y)^2
%Fdb= 1/N*sum (a*x^3+b*x^2+c*x+d-y)*x^2 =0 
%-> 1/N*sum (a*x^5+b*x^4+c*x^3+d*x^2-y*x^2) =0
%-> 1/N*sum (a*x^5+b*x^4+c*x^3+d*x^2) =1/N*sum (y*x^2)
sum21=0;
sum22=0;
sum23=0;
sum24=0;
sumy2=0;
for i=1:N
x(i) = i/Dn;
sum21=sum21+x(i)^5;
sum22=sum22+x(i)^4;
sum23=sum23+x(i)^3;
sum24=sum24+x(i)^2;
sumy2=sumy2+y_measure(i)*x(i)^2;
end

%F=min 0.5/N*(a*x^3+b*x^2+c*x+d-y)^2
%Fdc= 1/N*sum (a*x^3+b*x^2+c*x+d-y)*x =0 
%-> 1/N*sum (a*x^4+b*x^3+c*x^2+d*x-y*x) =0
%-> 1/N*sum (a*x^4+b*x^3+c*x^2+d*x) =1/N*sum (y*x)
sum31=0;
sum32=0;
sum33=0;
sum34=0;
sumy3=0;
for i=1:N
x(i) = i/Dn;
sum31=sum31+x(i)^4;
sum32=sum32+x(i)^3;
sum33=sum33+x(i)^2;
sum34=sum34+x(i);
sumy3=sumy3+y_measure(i)*x(i);
end

%F=min 0.5/N*(a*x^3+b*x^2+c*x+d-y)^2
%Fdd= 1/N*sum (a*x^3+b*x^2+c*x+d-y)*1 =0 
%-> 1/N*sum (a*x^3+b*x^2+c*x^1+d-y) =0
%-> 1/N*sum (a*x^3+b*x^2+c*x^1+d) =1/N*sum (y)
sum41=0;
sum42=0;
sum43=0;
sum44=0;
sumy4=0;
for i=1:N
x(i) = i/Dn;
sum41=sum41+x(i)^3;
sum42=sum42+x(i)^2;
sum43=sum43+x(i)^1;
sum44=sum44+1;
sumy4=sumy4+y_measure(i);
end

A=[sum11/N, sum12/N, sum13/N, sum14/N;
   sum21/N, sum22/N, sum23/N, sum24/N;
   sum31/N, sum32/N, sum33/N, sum34/N;
   sum41/N, sum42/N, sum43/N, sum44/N;];

b=[sumy1/N;
   sumy2/N;
   sumy3/N;
   sumy4/N];
p_guan=pinv(A)*b
N=50;
for i=1:N
x(i) = i/Dn;
y_estg(i)=p_guan(1)*x(i)^3+p_guan(2)*x(i)^2+p_guan(3)*x(i)+p_guan(4);
end

p_matlab=polyfit(x,y_measure,3);
y_estg_matlab = polyval(p_matlab, x);
figure(2)
plot(x,y_real,'-.r');
hold on;
plot(x,y_measure,':sb');
hold on;
plot(x,y_estg,'-k');
hold on;
plot(x,y_estg_matlab,'-m');
grid on;
legend('真实曲线','测量值','直接法','广义法')

最终的拟合系数a,b,c,d如下：

p_real =

   -0.5000
    1.0000
   -0.3000
    0.0300


p_d =

   -0.4653
    0.8887
   -0.1974
    0.0063


p_guan =

   -0.4653
    0.8887
   -0.1974
    0.0063

可以看到广义法和直接法拟合结果一样，但由于存在数据噪声与真实的参数都存在误差，此时可以通过增加拟合数据量来提高逼近精度，但从代码可以看到广义法最终的计算量仅为4X4矩阵求伪逆，而直接法的计算量会成倍增加。

同理给出拟合圆的例子如下，这里我们基于上面的流程图首先换元后采用广义最小二乘处理：

%%拟合圆
%x^2+y^2-2ax-2by+a^2+b^2=r^2
%制造测量
N=90;
Dn=N/2;
d_sampe=5/N;
w=0.15;
a=0.5;
b=-1;
r=0.8;
p_real_circle=[a;b;r]

t = linspace(0,2*pi,100);
xs=sin(t)*r+a;
ys=cos(t)*r+b;
for i=1:N
x(i) = xs(i);
y_real(i)=ys(i);
y_measure(i)=y_real(i)+randn(1)*w;
end

%%间接法
%换元A=-2a  B=-2a R=a^2+b^2-r^2  
%->x^2+y^2+Ax+By+R=0
Ac(1,:)=[x(1),y_measure(1),1];
bc(1,:)=[-(x(1)^2+y_measure(1)^2)];
for i=2:N
Ac=[Ac(:,:);
   x(i),y_measure(i),1];
bc=[bc(:,:);
   -(x(i)^2+y_measure(i)^2)];
end
p_temp=pinv(Ac)*bc;
a=-p_temp(1)/2;
b=-p_temp(2)/2;
r=(a^2+b^2-p_temp(3))^0.5;
p_d_circle=[a;b;r]

t = linspace(0,2*pi,100);
x_est=sin(t)*r+a;
y_est=cos(t)*r+b;

figure(3)
plot(xs,ys,'-.r');
hold on;
plot(x,y_measure,'s:b');
hold on;
plot(x_est,y_est,'-k');
grid on;
axis equal;
legend('真实曲线','测量值','直接法','广义法')

%%广义法
%换元A=-2a  B=-2a R=a^2+b^2-r^2  
%->x^2+y^2+Ax+By+R=0
%F=min 0.5/N*(x^2+y^2+Ax+By+R)^2
%Fda= 1/N*sum (x^2+y^2+Ax+By+R)*x =0 
%-> 1/N*sum (x^3+xy^2+Ax^2+Bxy+xR) =0
%-> 1/N*sum (Ax^2+Bxy+xR) =-1/N*sum (x^3+xy^2)
sum11=0;
sum12=0;
sum13=0;
sumy1=0;
for i=1:N
x(i) =xs(i);
sum11=sum11+x(i)^2;
sum12=sum12+x(i)*y_measure(i);
sum13=sum13+x(i);
sumy1=sumy1-(x(i)^3+x(i)*y_measure(i)^2);
end

%Fdb= 1/N*sum (x^2+y^2+Ax+By+R)*y =0 
%-> 1/N*sum (x^2*y+y^3+Axy+By^2+yR) =0
%-> 1/N*sum (Axy+By^2+yR) =-1/N*sum (x^2*y+y^3)
sum21=0;
sum22=0;
sum23=0;
sumy2=0;
for i=1:N
x(i) =xs(i);
sum21=sum21+x(i)*y_measure(i);
sum22=sum22+y_measure(i)^2;
sum23=sum23+y_measure(i);
sumy2=sumy2-(x(i)^2*y_measure(i)+y_measure(i)^3);
end

%FdR= 1/N*sum (x^2+y^2+Ax+By+R)*1 =0 
%-> 1/N*sum (x^2+y^2+Ax+By+R) =0
%-> 1/N*sum (Ax+By+R) =-1/N*sum (x^2+y^2)
sum31=0;
sum32=0;
sum33=0;
sumy3=0;
for i=1:N
x(i) =xs(i);
sum31=sum31+x(i);
sum32=sum32+y_measure(i);
sum33=sum33+1;
sumy3=sumy3-(x(i)^2+y_measure(i)^2);
end

Asg=[sum11/N, sum12/N, sum13/N;
   sum21/N, sum22/N, sum23/N;
   sum31/N, sum32/N, sum33/N;];

bsg=[sumy1/N;
   sumy2/N;
   sumy3/N];
p_guan_circle_temp=pinv(Asg)*bsg;

a=-p_guan_circle_temp(1)/2;
b=-p_guan_circle_temp(2)/2;
r=(a^2+b^2-p_guan_circle_temp(3))^0.5;
p_guan_circle=[a;b;r]

t = linspace(0,2*pi,100);
x_estg=sin(t)*r+a;
y_estg=cos(t)*r+b;

figure(4)
plot(xs,ys,'-.r');
hold on;
plot(x,y_measure,'s:b');
hold on;
plot(x_est,y_est,'-k');
hold on;
plot(x_estg,y_estg,'-m');
grid on;
axis equal;
legend('真实曲线','测量值','直接法','广义法')

上面首先采用换元处理之后基于广义法求取各变量均值并构建Ax=b的齐次方程组，进一步求取伪逆得到中间变量值，结合中间变量定义和圆的半径物理约束最终确定相关系数。

p_real_circle =

    0.5000
   -1.0000
    0.8000


p_d_circle =

    0.5026
   -0.9512
    0.8116

p_guan_circle =

    0.5026
   -0.9512
    0.8116

>> 

2. 线性最小二乘法的一些应用举例

上一小节中给出的间接法虽然能在一定程度上解决模型中存在的非线性项，但对于如多个系数相乘或者更高阶的非线性部分来说可能最终无法求取出结果，对于一些实际的应用问题可能需要一些针对性的处理反而能规避上述问题仍然使用最小二乘法来求解，下面给出机器人系统中几个常见可采用最小二乘法的案例：

（1）加速度计标定-直接法

加速度计标定的目的是消除IMU传感器各轴测量相对标准重力加速度的尺度偏差和零偏，另外如果各自传感器安装不正交还存在着正交偏差，我们假设不考虑该误差仅考虑尺度与零偏，则三轴加速度测量模型如下：

则由于有六个未知系数最少需要构建六个测量数据，因此在实际中我们也常常采用所谓的六面标定法来获取加速度标定数据，静止状态下，不管加速度计方向如何，其测量得到的加速度为重力加速度在各轴上的投影，即重力加速度向量模值总是等于g，则设各轴测量值单位也为g则有：

展开上式有：

可以看到上式中除了未知系数的二阶项外还有两个不同系数的相乘项，这样的非线性部分是无法直接使用换元的方法简单替换的，因此对于使用该方程的形式只能告一段落，如果要求解上式的非线性最小二乘问题可采用LM或Guass牛顿法来实现，这里我们需要采用另一种表达形式。

首先将策略模型写成矩阵形式有：

通过扩维将上式转换为Ax=b的齐次方程形式有：

等式两边同时转秩得到：

则采用6面标定法可以得到六组测量数据：

则同理由于矩阵A不为方阵采用最小二乘解（伪逆）的方式计算出系数矩阵，可见对于包含如上的非线性系统模型如任然想采用最小二乘的方法来求解需要针对性的特殊处理，没有统一的方法来确定是否换元还是扩维如加速度6面标定只是一个特例，对于更广泛的非线性最小二乘问题还是需要采用更正规的优化迭代方法实现。

（2）磁力计标定-间接法+广义法

在磁场拟合中由于磁力计受软磁和硬磁干扰导致测量的磁场数据为空间中的椭球，因此磁力计标定的思想就是通过采集的磁场测量数据拟合出该空间椭圆，进一步获取椭圆X-Y，Y-Z切面的长短轴参数与椭球球心偏差，通过尺度与偏差变换将其转换为标准正球体从而实现纠正，因此可以采用最小二乘法完成该拟合问题，则典型的空间椭球体标准方程如下：

则展开转换为一般式，并使用间接法替换非线性项：

所替换的非线性项为：

则基于广义法定义误差方程为：

并进一步建立最小二乘问题：

通过求取F对各中间系数的偏导，通过寻找极小值构建方程组，采用最小二乘解求取最佳中间系数，最后用其与椭球方程对应系数逆推得到：