机器人-数值分析法，求并联机器人的正解

本文链接：https://blog.csdn.net/carryoutw/article/details/144092956

最近公司在培训，学到了新东西，拿出来分析一下。

就是牛顿迭代（数值分析法求解），我们都知道对于并联机器人来说利用几何方法求解逆解是横简单的（前几期文章有推导），相对来说正解是非常困难的。用传统的解析（解方程的方式大大消耗控制器的算力），所以利用数值分析的方法来估计最终的结果。

【数值分析】速成牛顿迭代法｜考试宝典｜一定能听懂！！_哔哩哔哩_bilibili喜欢的话欢迎关注点赞投币收藏～谢谢大家！！, 视频播放量 234262、弹幕量 612、点赞数 5097、投硬币枚数 2727、收藏人数 4271、转发人数 994, 视频作者小西小西日记, 作者简介 wb同名立志要做数学区up的小菜鸡，相关视频：三小时速通数值分析（已完结），【数值分析】牛顿迭代法——求解非线性方程，【数值分析】轻松掌握梯形法改进欧拉法｜考试宝典｜速成，【数值分析】【纯干货】速成牛顿插值法，《数值分析》3小时期末不挂科！期末速成丨考前突击丨期末不挂科丨考点总结，简单迭代法的应用：求根式的近似值，牛顿迭代法和二分法求解非线性方程C语言实现，第五章最优化方法（6）最速下降法，牛顿法，阻尼牛顿法，西电微电子考研计算题迭代法—计算器一步到位，数值分析25-线性方程组迭代法：Gauss-seidel（例题）https://www.bilibili.com/video/BV1qv411M7AD?spm_id_from=333.788.videopod.sections&vd_source=c0f0446e22649eeb5ab253cb77c744c7通过视频可以知道，牛顿迭代的方法来求解非线性方程，得到是一个估计值。

牛顿迭代的公式是：

$x_{k+1} = x_k+\frac{f(x)}{\dot{f(x)}}$

其中，f（x）是根据原方程构造的一个函数，f(x)的导是对x进行求导。

同时通过前几个文章得到的detla并联机器人推导的正解公式。

对其使用牛顿迭代的方式进行求解，其中e是动平台和静平台的半径差，假设为50，l1和l2是主动臂和从动臂的长度，假设为300,600，belta是主动臂在静平台的位置，为假设为-30,90和210。（相邻差120度）

通过输入theta123的角度值，估算出xyz的值。

一步一步来，把已知条件带入并化简。

化简：

需要注意的是哈，在机器人中，对f(x)的x求导正好是求雅克比矩阵的方法之一，即：

类似的，求出三阶的雅克比矩阵的逆。

这里利用matlab进行计算，（在写程序时，可以先利用公式把雅克比矩阵的逆求出来，在进行运算）

逆解：

% 参数定义
l1 = 300; % 主动臂长度
l2 = 600; % 从动臂长度
e = 50;   % 外接圆半径差
x = 0; y = 0; z = -300; % 动平台位置
beta = [-pi/6, pi/2, 7*pi/6]; % 结构角（弧度）


% 初始化结果
theta = zeros(1, 3); % 用于存储每个支链的结果

% 逐个计算 E, F, G 和 theta
for i = 1:3
    % 计算 E, F, G
    Ei = 2 * l1 * z;
    Fi = -2 * l1 * (x * cos(beta(i)) - e + y * sin(beta(i)));
    Gi = x^2 + y^2 + z^2 - 2 * e * (x * cos(beta(i)) + y * sin(beta(i))) + l1^2 - l2^2 + e^2;
    % 计算 θ_i
    theta(i) = 2 * atan((-Ei - sqrt(Ei^2 + Fi^2 - Gi^2)) / (Fi - Gi));
end

% 将结果转为角度制
theta_deg = rad2deg(theta);

% 输出结果
disp('Theta (degrees):');
disp(theta_deg);

正解：

% 参数定义
l1 = 300; % 主动臂长度
l2 = 600; % 从动臂长度
e = 50;   % 外接圆半径差
beta = [-pi/6, pi/2, 7*pi/6]; % 结构角（弧度）

% 初始猜测值
x = 0; y = 0; z = 100; % 初始位置，假设在 (0, 0, 100)
theta_target = deg2rad(67.115); % 目标角度为 -20 度（弧度制）
epsilon = 1e-6; % 收敛条件
max_iter = 100; % 最大迭代次数

% 牛顿迭代
for iter = 1:max_iter
    % 计算方程 f1, f2, f3
    f1 = x^2 + y^2 + z^2 - 2*(l1*cos(theta_target) + e)*(x*cos(beta(1)) + y*sin(beta(1))) ...
         + 2*z*l1*sin(theta_target) + 2*e*l1*cos(theta_target) + l1^2 - l2^2 + e^2;
    f2 = x^2 + y^2 + z^2 - 2*(l1*cos(theta_target) + e)*(x*cos(beta(2)) + y*sin(beta(2))) ...
         + 2*z*l1*sin(theta_target) + 2*e*l1*cos(theta_target) + l1^2 - l2^2 + e^2;
    f3 = x^2 + y^2 + z^2 - 2*(l1*cos(theta_target) + e)*(x*cos(beta(3)) + y*sin(beta(3))) ...
         + 2*z*l1*sin(theta_target) + 2*e*l1*cos(theta_target) + l1^2 - l2^2 + e^2;

    F = [f1; f2; f3];

    % 计算雅可比矩阵
    J = [
        2*x - 2*(l1*cos(theta_target) + e)*cos(beta(1)), ...
        2*y - 2*(l1*cos(theta_target) + e)*sin(beta(1)), ...
        2*z + 2*l1*sin(theta_target);

        2*x - 2*(l1*cos(theta_target) + e)*cos(beta(2)), ...
        2*y - 2*(l1*cos(theta_target) + e)*sin(beta(2)), ...
        2*z + 2*l1*sin(theta_target);

        2*x - 2*(l1*cos(theta_target) + e)*cos(beta(3)), ...
        2*y - 2*(l1*cos(theta_target) + e)*sin(beta(3)), ...
        2*z + 2*l1*sin(theta_target)
    ];

    % 计算增量
    delta = -inv(J) * F; % 显式计算增量
    
    % 更新解
    x = x + delta(1);  % x_n+1 = x_n + delta_x
    y = y + delta(2);  % y_n+1 = y_n + delta_y
    z = z + delta(3);  % z_n+1 = z_n + delta_z
    
    % 检查收敛条件
    if norm(delta) < epsilon
        fprintf('收敛于第 %d 次迭代\n', iter);
        break;
    end
end

% 打印最终结果
fprintf('最终结果: x = %.4f, y = %.4f, z = %.4f\n', -x, -y, -z);

if iter == max_iter
    fprintf('未能在最大迭代次数内收敛\n');
end

结果对比：