机器人动力学拉格朗日乘子法求解动力学方程

clear
clcsyms m1 m2 m3 real
syms L1 L2 L3 h1 h2 h3 w1 w2 w3 real
syms theta1 theta2 theta3 real
syms g real%% 初始化 需要自己修改的地方
% DH 参数
DH=[0,0,0,theta1;0,L1,0,theta2];    % DH参数
FreeV = [theta1,theta2];             % 自由变量
[Linen,covn] = size(DH);
dof = Linen;                        % 自由度
% 伪惯性矩矩阵
I1 = [m1*(L1^2)/3 0 0 m1*L1/2;0 0 0 0; 0 0 0 0;m1*L1/2 0 0 m1];
I2 = [m2*(L2^2)/3 0 0 m2*L2/2;0 0 0 0; 0 0 0 0;m2*L2/2 0 0 m2];
I = cat(3,I1,I2);
% 质心位置
C1 =  [1/2*L1 0 0 1];
C2 =  [1/2*L2 0 0 1];
C = cat(3,C1,C2);
% 重力加速度
G0 = [0,-g,0,0];
% 连杆质量
M = [m1,m2];
%% 0-i 变换矩阵
T = sym(zeros(4,4,dof));
for i = 1:dofif (i == 1)T(:,:,i) = simplify(generateTransFor(DH(i,1),DH(i,2),DH(i,3),DH(i,4)));elseT(:,:,i) = simplify(T(:,:,i-1) * generateTransFor(DH(i,1),DH(i,2),DH(i,3),DH(i,4)));end
end
%% 求变换矩阵的一阶导数 DT每一行是从0到n的变换矩阵 每一列是求导
DT = sym(zeros(4,4,dof,dof));
for i = 1:doffor j = 1:dofDT(:,:,i,j) = simplify(diff(T(:,:,i),FreeV(1,j)));end
end
%% DT2 第三维是 0到n的变换矩阵 第四维是第一次求导 第五维度是第二次求导
DT2 = sym(zeros(4,4,dof,dof,dof));
for i = 1:doffor j = 1:doffor m = 1:dofDT2(:,:,i,j,m) = simplify(diff(DT(:,:,i,j),FreeV(1,m)));endend
end
%% 计算D
D = sym(zeros(dof,dof));
for i = 1:doffor j = 1:doffor m = max(i,j):dofD(i,j) = simplify(D(i,j)+trace(DT(:,:,m,i)*I(:,:,m)*DT(:,:,m,j)'));endend
end
%%  计算H
H = sym(zeros(dof,dof,dof));
for i = 1:doffor k = 1:doffor m = 1:doffor n = max([i,k,m]):dofH(i,k,m) = simplify(H(i,k,m)+trace(DT(:,:,n,i)*I(:,:,n)*DT2(:,:,n,k,m)'));endendend
end
H = reshape(H,dof,dof*dof);
%%    计算G
G = sym(zeros(1,dof));
for i = 1:doffor k = i:dofG(1,i) = simplify(G(1,i)+M(1,k)*G0*DT(:,:,k,i)*C(:,:,k)');end
end
G = reshape(G,dof,1);
%%
symdisp(D);
symdisp(H);
symdisp(G);

机器人动力学拉格朗日乘子法求解动力学方程相关推荐

约束优化方法之拉格朗日乘子法与KKT条件
来源:https://www.cnblogs.com/ooon/p/5721119.html 引言本篇文章将详解带有约束条件的最优化问题,约束条件分为等式约束与不等式约束,对于等式约束的优化问题,可 ...
拉格朗日乘子法 KKT条件
目录 1. 拉格朗日乘子法用于最优化的原因 2. 最优化问题三种情况 2.1 无约束条件 2.2 等式约束条件:拉格朗日乘子法 2.3 不等式约束条件:KKT 3. Lagrange对偶函数 3.1 ...
基础数学知识（一）——拉格朗日乘子法
这几天一直在看支持向量机,然后就是大量大量的数学公式,一直迷迷糊糊的,然后一直遇到拉格朗日,拉格朗日,原来数学基础也不好,没怎么学过,于是下定决心要把拉格朗日乘子法搞懂,花了几天,看了一些文章,算是对 ...
【精简推导】支持向量机（拉格朗日乘子法、对偶函数、KKT条件）
支持向量机,就是通过找出边际最大的决策边界,来对数据进行分类的分类器.因此,支持向量分类器又叫做最大边际分类器. (疯狂暗示:这是一个最优化问题啊~) 直接上目标求解函数: 这个式子是支持向量机基本形 ...
matlab增广拉格朗日,[Opt] 拉格朗日乘子法 | ADMM | Basis pursuit
乘子法本文先简要介绍三个乘子法,它们的收敛条件依次减弱(不做具体介绍),然后应用 ADMM 算法求解 Basis pursuit 问题最后读读 Boyd 给出的代码. Lagrange Multip ...
机器学习知识点(六)增广矩阵求解拉格朗日乘子法的Java实现
基本的拉格朗日乘子法就是求函数f(x1,x2,...)在g(x1,x2,...)=0的约束条件下的极值的方法.其主要思想是将约束条件函数与原函数联系到一起,使能配成与变量数量相等的等式方程,从而求出得 ...
求解最优化问题的方法：拉格朗日乘子法（Lagrange Multiplier) 和KKT条件
在求解最优化问题中,拉格朗日乘子法(Lagrange Multiplier)和KKT(Karush Kuhn Tucker)条件是两种最常用的方法.在有等式约束时使用拉格朗日乘子法,在有不等约束时使用 ...
最优化：拉格朗日乘子法
作者:桂. 时间:2017-03-27 20:26:17 链接:http://www.cnblogs.com/xingshansi/p/6628785.html 声明:欢迎被转载,不过记得注明出处哦~ ...
拉格朗日乘子法学习笔记
本来是想写支持向量机的学习笔记的然后觉得内容太多了越写越不想写于是咕掉了. 把写好的拉格朗日乘子法发上来吧QwQ 拉格朗日乘子法 wiki链接拉格朗日乘子法用来求解带多个等式约束的情况下的多元函数极 ...

机器人动力学拉格朗日乘子法求解动力学方程

机器人动力学拉格朗日乘子法求解动力学方程相关推荐

最新文章

热门文章

机器人动力学 拉格朗日乘子法求解动力学方程

机器人动力学 拉格朗日乘子法求解动力学方程相关推荐

最新文章

热门文章

机器人动力学拉格朗日乘子法求解动力学方程

机器人动力学拉格朗日乘子法求解动力学方程相关推荐