梯度下降法，常被用来求解算法的最佳参数(例如求解神经网络中的权重和偏置)，举个多元线性回归问题的例子，来解释一下梯度下降法的原理

预测城市的平均房价y与该城市的GDP(x1),人口数(x2),交通便利程度(x3)等有关，X的权重矩阵用W来表示。

其中求解预测的最佳参数W的过程可以用梯度下降法进行参数的最优化。

如果用最小二乘法来计算误差J=(XW-Y)^2 ，令J对W偏导数=0，求解误差J最小时W的值，解如下，这种情况下有局限性的前提是X为可逆矩阵（也就是说X不含有多重共线性）。

而梯度下降法则是一种更广泛的求解权重W的方法，很多资料中讲误差J比作是一个山谷，在山顶向山谷里丢一个人进去，其下降到无法下降的时候我们也就找到了山谷（也就是误差）的最低点，

在最低点的梯度（导数值）也就为0。将人不断下降的过程看作是按照一定横向的步长a（也叫做学习率）迭代的过程，为了方便后续求解我们在误差J前面加一个1/2m的常数，由于最后梯度会变为0，就会停止迭代了。

那么其中的梯度到底是什么？

梯度其实叫雅各比矩阵(Jacobian matrix) ，实际上就是多变量微分的一般化，在多变量函数中，梯度是一个向量，可以理解成多元函数在某一点的各个方向导数所构成的一个矩阵，它体现了一个可微方程与给出点的最优线性逼近。

如何应用梯度下降法求解多元线性回归问题呢？

把初始问题中的权重矩阵进行设置，其中gradient_descen这个函数是求解雅各比矩阵。

import numpy as np
 
# 数据的个数
m = 5
 

X0 = np.ones((m, 1))
X1 = np.array([
    90,89,80, 100,100,140,100,100,120, 100,120, 150, 112,150,200
]).reshape(m, 3)
X = np.hstack((X0, X1))
#房价的三个特征

y = np.array([
    171,236,217,256,328
]).reshape(m, 1)
 
#学习的步长 学习率
alpha = 0.000001
#误差函数
def error_function(theta, X, y):
    diff = np.dot(X, theta) - y
    return (1./2*m) * np.dot(np.transpose(diff), diff)
#偏导数函数 雅各比矩阵
def gradient_function(theta, X, y):
    diff = np.dot(X, theta) - y
    return (1./m) * np.dot(np.transpose(X), diff)
#梯度下降函数求解权重的函数
def gradient_descent(X, y, alpha):
    new_inter=0
    theta = np.array([1,1, 1,1]).reshape(4, 1)
    gradient = gradient_function(theta, X, y)
    while (np.all(np.absolute(gradient) >= 1e-5) ):
        #print(theta)
        theta = theta - alpha * gradient
        new_inter=new_inter+1
        gradient = gradient_function(theta, X, y)
    return theta
optimal = gradient_descent(X, y, alpha)
print('optimal:', optimal)
print('error function:', error_function(optimal, X, y)[0,0])

b和三个权重的权重分别为，0.99 0.44 0.65 0.89 误差项在10^1的数量级，在使用的时候应该注意调整学习率的参数，防止过拟合或无法完成迭代，会报出现缺失值的error

导入

在基于图像的视觉伺服中，我们可以通过相机观察到图像在相机中的位置发生变化 ，针对于每个像素来说呢？便是图像中像素发生了位置变化，通过求导我们可以将位置的变化转化成像素的速度变化。那有没有一种东西可以将像素的速度变化转变到相机坐标系下呢？答案是有的。它就是图像的雅各比矩阵或者严格意义上来说是交互矩阵。那交互矩阵又是怎么将像素速度的变化与相机速度的变化联系到一起的呢？下面想通过推导来说明一下这个问题。

符号定义

根据相机成像模型，我们做如下定义：

话外：像素是什么呢？通过百度和wiki我们可以看到定义：

像素，为影像显示的基本单位，译自英文“pixel”，每个这样的消息元素不是一个点或者一个方块，而是一个抽象的取样。照片是一个个取样点的集合，在影像没有经过不正确的/有损的压缩或相机镜头合适的前提下，单位面积内的像素越多代表分辨率越高，所显示的影像就会接近于真实物体。

相机所说的像素，其实是最大像素的意思，像素是分辨率的单位，这个像素值仅仅是相机所支持的有效最大分辨率。30万 640×480=307,200像素 我懵了。

像素坐标系 u，v：位于像素平面上,其原点位于图像左上角,单位为像素。
图像坐标系 x，y：位于成像平面上,原点在在成像平面的中心点附近,单位为物理单位(如 mm)。
相机坐标系 X，Y，Z：原点为光心,单位为物理单位(如 mm)。
世界坐标系 :实际三维空间中的坐标系,根据使用需要进行定义,是用于表示系统中绝对位置的坐标系,单位为物理单位(如 mm)。

为了表达图像中被测量的特征（单一点特征）：
:图像中可以被测量的图像特征值向量
：图像中被测量的图像特征速度向量

如果是单一的图像点，我们将有

图像特征速度和相机速度线性相关，为此定义相机速度：
:代表相机速度向量,包含线速度v和角速度w

图像特征速度向量和相机速度向量，存在下面关系：

矩阵L被称作交互矩阵或图像的雅各比矩阵，严格来说，交互矩阵并不是图像雅各比矩阵，因为相机速度ξ 并不是位姿参数的偏导

:表示相机坐标在时间t时刻相对于固定坐标系o的旋转量

:表示相机坐标在时间t时刻相对于固定坐标系o的位置变化

：空间中一个固定点

：对应空间中的点p投影到图像中的特征向量

接下来我们的目标便是通过一系列代数推导获得相机速度ξ 与图像特征点（空间中的点投影到图像平面点的偏导）速度˙s的交互矩阵L。最终得出˙s = Lξ。

固定点相对于移动相机的速度

我们定义

作为点P相对于世界坐标系o的坐标。记住P并不会随时间变化，因此它相对于世界坐标系是固定的。

如果我们定义

作为在t时刻，P相对于移动相机坐标系的坐标。

结合之前的定义，便有

因此，在t时刻，P相对于移动相机的坐标便可以表示为

现在我们对这个公式求导

其中

从移动坐标系到固定坐标点P的向量。表达了相对于固定坐标系的坐标，因此

从移动坐标系到固定坐标点P的向量。表达了相对于移动坐标系的坐标。

利用角速度与旋转矩阵偏导的关系，这里的S(ω(t))是斜对称矩阵，这一关系的推导，这里暂时不做说明。

我们可以转化公式为

其中变量ω代表了移动坐标系相对于固定坐标系的角速度。即ω = ω0，因此，说明一下这一块多了一个R^T

给出移动坐标系相对于移动坐标系的角速度。
类似的

利用上面等式，我们可以立刻得到P相对于移动相机坐标系的速度。

很有趣的一个结论是，一个固定坐标点相对于移动坐标系的速度是一个移动相对于固定坐标点速度的-1倍。

构建交互矩阵

为了简化说明。定义P点坐标相对于相机坐标为

P相对于移动坐标系的速度是

定义角速度向量

进一步简化

使用上面的定义，我们可以将等式

转变成

利用向量叉乘，可以写成

假设图像投影可以通过透视投影被建模，

通过三角形相似的原理，我们可以表达空间中的点P的x和y坐标与图像平面的u，v和深度z的表达式为

其中λ为相机的焦距。

带入到上一个等式中，我们可以得到

这些等式可以用来表达速度˙p 对于图像平面的u，v以及点深度z和相机角速度和线速度的关系。这里的目的是为了将图像平面速度( ˙u, ˙v)T（转置）到相机坐标的角速度和线速度联系到一起。下面我们要将( ˙u, ˙v)T 与上面的等式进行联系。

对上面的投影等式求偏导，我们可以得到

然后将x，y，z的偏导公式带入到上式中，我们可以得到

采用同样的方法，我们可以得到


将上面两个等式写到一起，我们便可以得到

通过这个矩阵等式，我们可以得到一个点的交互矩阵。它将相机的速度与点在图像平面的投影速度联系到了一起。注意这个交互矩阵是关于图像点坐标u，v以及点在相机坐标下的深度z。因此，这个等式可以简化为

到这里，我们已经将图像平面的像素速度与相机速度进行关联的交互矩阵进行了推导。

六轴六自由度机械臂轨迹跟踪的matlab实现（基于速度雅各比矩阵方法）

对于六轴六自由度机械臂进行轨迹规划，并针对其设计滑模控制器，实现机械臂的末端轨迹跟踪。（完整代码链接见文章末尾）
本文所用机械臂为innfos-Gluon-6L3，通过standard DH方法建模得到参数如下：

本文利用速度雅各比矩阵的方法来实现轨迹跟踪。这一方法的优点在于可以完全避免逆运动学求解，更加节省时间。

1.轨迹跟踪的控制结构图设计

控制系统的输入，同样也是系统的期望输出，是机械臂的目标位姿$x_d=(x,y,z,\alpha ,\beta ,\gamma )$。其刚好对应机械臂的六个自由度。前三者是机械臂末端坐标系相对于世界坐标系的位置，后三者是机械臂末端坐标系相对于世界坐标系的旋转角度。每一时刻的机械臂期望位姿都是已知的，它是通过轨迹规划得到的，这一点将在后文详细讲解。
系统的控制器采用滑模控制器，其输入为位姿误差$e=x_d-x$，输出为角度增量$\dot{q}$。
系统的被控对象是利用速度雅各比矩阵来建模的，其关系为$v=J(q)\dot{q}$。其中$J(q)$为速度雅各比矩阵。
系统的输出为机械臂的实际末端位置$x=\int vdt$。

2.系统的输入：轨迹规划

为了使得机械臂的末端运动平滑，常对机械臂进行规划，对末端速度、加速度等进行一定约束。
工业上常使用七段式S型曲线来进行轨迹规划，其示意图图如下：

其有七个等间距时间段，分别为加加速段、加速度恒定段、加减速段、匀速段、减减速段、加速度恒定段、减加速段。
各段速度的计算表达式如下，其中$J$为加加速度。

式中的$v_{max}$等参数的计算推导如下：
$$\begin{gathered} △T=t_{i+1}-t_i=\frac{t_f-t_0}{7} \\ L=4v_{max}△T \\ {v}_2=v_1+a_{max}△T \\ {a}_{max}=\frac{2v_1}{△T} \\ {v}_{max}=v_1+v_2=2v_1+a_{max}△T=4v_1 \\ L=16v_1△T\to v_1=\frac{L}{16△T} \end{gathered}$$
所以最终只需知道末端位移$L$和所需时间$T$，即可计算得到整个轨迹规划曲线。其matlab代码实现如下。
其中的$x\_s$、$y\_s$、$z\_s$分别为每时的机械臂末端位置，即期望期望位姿$x\_d$的前三行。$x\_d$的后三行为末端位置的姿态信息，在本次仿真中我们默认机械臂的姿态始终为0，所以$x\_d$的后三行总是为0。

%% 0.设定初始参数
% xyz_start=[0,-120.02,533.96];       %轨迹起点(关节角为0)的末端坐标，单位mm；
xyz_start=[0,-120.02,533.96];
xyz_end=[-30,-45,385];     %轨迹终点的末端坐标；
T=10;       %完成轨迹规划的时间；

%% 1.轨迹规划
L=sqrt((xyz_end(1)-xyz_start(1))^2+(xyz_end(2)-xyz_start(2))^2+(xyz_end(3)-xyz_start(3))^2);
dt=T/7;        %每段的时间长度
v1=L/(16*dt);   %第一次加速度拐点
J=2*v1/(dt*dt); %加加速度
amax=dt*J;      %最大加速度
v2=v1+dt*amax;  %第二次加速度拐点
vmax=v2+v1;     %第三次速度拐点

t1 = 1*dt;
t2 = 2*dt;
t3 = 3*dt;
t4 = 4*dt;
t5 = 5*dt;
t6 = 6*dt;
t7 = 7*dt;

t=0:0.1:T;

vt1=1/2*J*t.^2.*(t>=0 & t<t1);
vt2=(v1+amax*(t-t1)).*(t>=t1 & t<t2);
vt3=(vmax-1/2*J*(t3-t).^2).*(t>=t2 & t<t3);
vt4=vmax.*(t>=t3 & t<t4);
vt5=(vmax-1/2*J*(t-t4).^2).*(t>=t4 & t<t5);
vt6=(v2-amax*(t-t5)).*(t>=t5 & t<t6);
vt7=(1/2*J*(t7-t).^2).*(t>=t6 & t<t7);

vt=vt1+vt2+vt3+vt4+vt5+vt6+vt7;     %各时刻速度

S=zeros(1,length(t));       %各时刻位移
for i=2:length(t)
    S(i)=trapz(t(1:i),vt(1:i));
end 

%各时刻xyz的位移
x_s=xyz_start(1)+(xyz_end(1)-xyz_start(1))/L*S;     
y_s=xyz_start(2)+(xyz_end(2)-xyz_start(2))/L*S;
z_s=xyz_start(3)+(xyz_end(3)-xyz_start(3))/L*S;

%各时刻xyz轴的速度分量
v_x=(xyz_end(1)-xyz_start(1))/L*vt;
v_y=(xyz_end(2)-xyz_start(2))/L*vt;
v_z=(xyz_end(3)-xyz_start(3))/L*vt;

3.被控对象：速度雅各比矩阵

速度雅各比矩阵方法的关系表达式如下：
$$v=J(q)\dot{q}$$
前者$v$是末端执行器的速度，后者$\dot{q}$是关节角速度。表达式的物理意义是：当关节角度发生一个微小的变化$△q$,末端执行器也会相应产生一个微小的位姿变化$△x$。
速度的雅各比矩阵的求解方法有多种，如1.向量积方法 2.微分法 等等…
本文采用向量积的方法，求解方法如下。

$$\begin{gathered} J(q)=\left[\begin{array}{c}{J}_v\\ J_w\end{array}\right]=\left[\begin{array}{cccccc}{J}_1& J_2& J_3& J_4& J_5& J_6\end{array}\right] \\ {J}_i=\left[\begin{array}{c}{Z}_{i-1}\times r_{i-1}\\ Z_{i-1}\end{array}\right]=\left[\begin{array}{c}{Z}_{i-1}\times (P_n-P_{i-1})\\ Z_{i-1}\end{array}\right] \end{gathered}$$
本文采用的机械臂有6个关节角，因此其速度雅各比矩阵有6列，分别为$J_i$。本文的机械臂有6个自由度，因此对应的矩阵为6行。
每个雅各比矩阵分量$J_i$的后三行为 { i − 1 } \{i-1\} {i−1}坐标系相对于世界坐标系的$Z$轴分量；分量$J_i$的前三行为$Z_{i-1}$与$r_{i-1}$的差乘，$r_{i-1}$是末端坐标系与 { i − 1 } \{i-1\} {i−1}坐标系的相对位置在世界坐标系中的表示。
这种方法求解只适用于standard DH方法建模的模型，若使用modify DH方法建模，则需对上式的下标做一定修改。
此方法的matlab代码实现如下：

function [ J ] = Jacob_cross_SDH( q )
%JACOB_CROSS_SDH 函数摘要
%   输入q0为逼近角，单位为弧度，矩阵大小1*6;
%   输出J为速度雅各比矩阵，矩阵大小6*6；
%   说明：利用向量积的方法求解系统的雅各比矩阵，方法1和方法2任选一种
%   说明：此求解方法基于SDH参数建模，若MDH方法建模，需进行一定的下标改动

d=[105.03,0,0,75.66,80.09,44.36];
a=[0,-174.42,-174.42,0,0,0];
alp=[pi/2,0,0,pi/2,-pi/2,0];
offset=[0,-pi/2,0,-pi/2,0,0];
thd=q+offset;

% 求各个关节间的变换矩阵
T0=trotz(0)*transl(0,0,0)*trotx(0)*transl(0,0,0);
T1=trotz(thd(1))*transl(0,0,d(1))*trotx(alp(1))*transl(a(1),0,0);
T2=trotz(thd(2))*transl(0,0,d(2))*trotx(alp(2))*transl(a(2),0,0);
T3=trotz(thd(3))*transl(0,0,d(3))*trotx(alp(3))*transl(a(3),0,0);
T4=trotz(thd(4))*transl(0,0,d(4))*trotx(alp(4))*transl(a(4),0,0);
T5=trotz(thd(5))*transl(0,0,d(5))*trotx(alp(5))*transl(a(5),0,0);
T6=trotz(thd(6))*transl(0,0,d(6))*trotx(alp(6))*transl(a(6),0,0);

% 求各个关节相对于惯性坐标系的变换矩阵
T00 = T0;
T01 = T1;
T02 = T1*T2;
T03 = T1*T2*T3;
T04 = T1*T2*T3*T4;
T05 = T1*T2*T3*T4*T5;
T06 = T1*T2*T3*T4*T5*T6;

% 求各个关节相对于末端坐标系的变换矩阵
T06 = T1*T2*T3*T4*T5*T6;
T16 = T2*T3*T4*T5*T6;
T26 = T3*T4*T5*T6;
T36 = T4*T5*T6;
T46 = T5*T6;
T56 = T6;

% 提取各变换矩阵的旋转矩阵
R00 = t2r(T00);
R01 = t2r(T01);
R02 = t2r(T02);
R03 = t2r(T03);
R04 = t2r(T04);
R05 = t2r(T05);
R06 = t2r(T06);

% 取旋转矩阵第3列，即Z轴方向分量
Z0 = R00(: , 3);
Z1 = R01(: , 3);
Z2 = R02(: , 3);
Z3 = R03(: , 3);
Z4 = R04(: , 3);
Z5 = R05(: , 3);
Z6 = R06(: , 3);

%% Method.1
% 求末端关节坐标系相对于前面各个坐标系的位置，即齐次变换矩阵的第四列
% pi6为坐标系i和末端坐标系的相对位置在坐标系i下的表示
P06 = T06(1:3, 4);
P16 = T16(1:3, 4);
P26 = T26(1:3, 4);
P36 = T36(1:3, 4);
P46 = T46(1:3, 4);
P56 = T56(1:3, 4);
P66 = [0; 0; 0];

% 使用向量积求出雅可比矩阵
% R0i为坐标系0到坐标系i的旋转矩阵
% R0i*Pi6指坐标系i和末端坐标系的相对位置在0坐标系下的表示
J1 = [cross(Z0, R00*P06); Z0];
J2 = [cross(Z1, R01*P16); Z1];
J3 = [cross(Z2, R02*P26); Z2];
J4 = [cross(Z3, R03*P36); Z3];
J5 = [cross(Z4, R04*P46); Z4];
J6 = [cross(Z5, R05*P56); Z5];

%% Method.2

% % pi为坐标系i与世界坐标系0的相对位置
% p0=transl(T00);
% p1=transl(T01);
% p2=transl(T02);
% p3=transl(T03);
% p4=transl(T04);
% p5=transl(T05);
% p6=transl(T06);
% 
% % p6-pi为i坐标系指向末端坐标系的向量
% % p6-pi即为末端坐标系与i坐标系相对位置在世界坐标系中的表示
% % Ji=[Jv;Jw]    对应六自由度的速度分量和旋转分量
% J1 = [cross(Z0, p6-p0); Z0];
% J2 = [cross(Z1, p6-p1); Z1];
% J3 = [cross(Z2, p6-p2); Z2];
% J4 = [cross(Z3, p6-p3); Z3];
% J5 = [cross(Z4, p6-p4); Z4];
% J6 = [cross(Z5, p6-p5); Z5];


J = [J1, J2, J3, J4, J5, J6];

end

4.控制器：等速率趋近的滑模控制器

控制器的输入为位姿误差$e=x_d-x$,输出为关节角的增量$\dot{q}$，因此控制器满足关系：
$$u=\dot{q}=f\_SMC(e)$$
为此需求设计滑模控制器$f\_SMC$。
设计滑模面：
$$\begin{gathered} s=ce \\ e=x_d-x \end{gathered}$$
设计趋近率为等速趋近率：
$$\dot{s}=-\xi sgns$$
推导得到控制器输出$u$：
$$\begin{gathered} \dot{s}=c\dot{e}=-\xi sgns \\ \dot{e}={\dot{x}}_d-\dot{x}=-\frac{1}{c}\xi sgns \\ v=v_d+\frac{1}{c}\xi sgns \\ u=\dot{q}=J^{-1}(q)v=J^{-1}(v_d+\frac{1}{c}\xi sgns) \end{gathered}$$
matlab代码实现如下：

dth = [0; 0; 0; 0; 0; 0];
th = [0; 0; 0; 0; 0; 0];

x=[xyz_start';0;0;0];        %其实时刻的位姿

lamda=1;		%阻尼矩阵的系数

k = 0.1;
ita = 0.0002;
c = 5;

e = [0; 0; 0; 0; 0; 0];
de = [0; 0; 0; 0; 0; 0];
for i = 1 : length(t)
    xd=[x_s(i);y_s(i);z_s(i);0;0;0];     %期望位姿
    dxd=[v_x(i);v_y(i);v_z(i);0;0;0];	%期望速度

    q=th(:, i);

    Jac = Jacob_cross_SDH(q');	%求解当前角度下的雅可比矩阵

    e(:, i) = xd - x(:,i);      %误差
    s = c*e(:, i);      %滑模面
    v=dxd + (1/c)*ita*sign(s);    %机械臂的末端实际速度
    
    de(:, i) = dxd - v;     %误差的微分
    dth(:, i) = inv(Jac+lamda.*diag(ones(1,6)))*v;      %关节角的增量
    th(:, i + 1) = th(:, i) + dth(:, i)*0.1;    %下一时刻的关节角度
    x(:, i+1) = x(:, i) + v*0.1;    %机械臂末端实际位姿
end

更多更高级的滑模控制器设计请点击博主的控制器设计github仓库。

5.结果展示

设定轨迹跟踪起始点和终点：
$$\left[\begin{array}{c}0\\ -120.02\\ 533.96\end{array}\right]\to \left[\begin{array}{c}-30\\ -45\\ 385\end{array}\right]$$
$10s$内的末端轨迹位置在各坐标轴的映射：

$10s$内系统跟踪末端轨迹位置的误差：

$10s$内各个关节角的角度：

源代码下载链接：https://github.com/Fantasty9413/Trajectory-tracking-