柚子快報(bào)邀請(qǐng)碼778899分享：5.27機(jī)器人基礎(chǔ)-機(jī)械臂逆解
柚子快報(bào)邀請(qǐng)碼778899分享：5.27機(jī)器人基礎(chǔ)-機(jī)械臂逆解
前面簡(jiǎn)單講了機(jī)械臂的正解問題，即通過原位姿和控制各關(guān)節(jié)的角度得到終點(diǎn)位姿。而在實(shí)際應(yīng)用的時(shí)候，我們通常都是知道起始點(diǎn)和末端終點(diǎn)的位姿，需要考慮如何達(dá)到，即運(yùn)動(dòng)學(xué)機(jī)械臂的運(yùn)動(dòng)學(xué)逆解問題。
求解操作臂運(yùn)動(dòng)學(xué)方程是一個(gè)非線性問題。我們需要考慮解的存在性、多重解性以及求解方法。
?1.解的存在性
解是否存在的問題完全取決于操作臂的工作空間。工作空間是操作臂末端執(zhí)行器所能達(dá)到的范圍。如果解存在，則被指定的目標(biāo)點(diǎn)必須在工作空間內(nèi)。靈巧工作空間是指機(jī)器人的末端執(zhí)行器能后從各個(gè)方向達(dá)到的空間區(qū)域，可達(dá)工作區(qū)間是機(jī)器人至少從一個(gè)方向上有一個(gè)方位可以達(dá)到的空間。靈巧工作空間是可達(dá)工作空間的子集。
2.多重解問題
在求解過程中可能遇到另一個(gè)問題就是多重解問題。
解的選擇標(biāo)準(zhǔn)。最短形成？權(quán)重？干涉？
?3.解法
?代數(shù)解法
二幅角反切公式：
atan2_百度百科
?
?幾何解
4.MATLAB?實(shí)操
這真的是一件難過的事情，在求逆解的時(shí)候我曲曲折折煩躁不已，還是沒有很好地求出來，還是參考了其他同學(xué)的代碼。本來想通過控制幾何關(guān)系來求解，但是應(yīng)該是一堆bug，已經(jīng)無力修改了，如果有人幫忙看看問題是什么就更好了
?
clear; clc;
%% 參數(shù)
L1 = 100;
L2 = 105;
L3 = 98;
L4 = 245;
IN_theta = [0, -45, 30,10, 0];
% DH 參數(shù)
C_a = [0, 0, L2, L3, 0, 0, 0];
C_d = [0, L1, 0, 0, 0, 0, L4];
C_alpha = [0, -90, 0, 0, -90, 0, 0];
C_theta = [0, IN_theta(1), IN_theta(2), IN_theta(3), IN_theta(4)-90, IN_theta(5), 0];
T_target = eye(4); % 初始化結(jié)果為單位矩陣
for i = 1:6
T = [cosd(C_theta(i+1)), -sind(C_theta(i+1)), 0, C_a(i);
sind(C_theta(i+1))*cosd(C_alpha(i)), cosd(C_theta(i+1))*cosd(C_alpha(i)), -sind(C_alpha(i)), -sind(C_alpha(i))*C_d(i+1);
sind(C_theta(i+1))*sind(C_alpha(i)), cosd(C_theta(i+1))*sind(C_alpha(i)), cosd(C_alpha(i)), cosd(C_alpha(i))*C_d(i+1);
0, 0, 0, 1]; % 根據(jù)給定的公式計(jì)算 T[i]
T_target = T_target * T; % 乘以每個(gè) T[i]
end
% 提取目標(biāo)位置
x_target = T_target(1, 4);
y_target = T_target(2, 4);
z_target = T_target(3, 4);
% 初始化最優(yōu)解
best_theta = [];
min_error = inf;
%% 逆運(yùn)動(dòng)學(xué)求解
for apha1 = -90:1:90 % L4與x軸的夾角，角度步長(zhǎng)為1度
% 方程組定義
syms theta2 theta3 theta4
% 幾何約束條件
eq1 = L4 * cosd(apha1) + L3 * cosd(apha1 + theta4) + L2 * cosd(theta2) == x_target;
eq2 = L1 + L4 * sin(apha1) + L3 * sind(apha1 + theta4) + L2 * sind(theta2) == z_target;
eq3 = theta2 == apha1 + theta3 + theta4;
%eq4 = cos(2/pi + theta2 + theta4 + deg2rad(apha1)) == (L2^2 + L4^2 - (L3 * cosd(apha1 + theta4) + L2 * cosd(theta2))^2 + (L3 * sind(apha1 + theta4) + L2 * sind(theta2))^2) / (2 * L2 * L3);
% 幾何約束的非負(fù)性
%condition1 = L1 + L2 * cosd(theta2) > 0;
%condition2 = L1 + L3 * cosd(apha1 + theta4) + L2 * cosd(theta2) > 0;
% 組合所有方程和約束條件
equations = [eq1, eq2, eq3];
% 求解方程組
solutions = solve(equations, [theta2, theta3, theta4], 'Real', true);
% 提取并驗(yàn)證解
theta2_sol = double(solutions.theta2);
theta3_sol = double(solutions.theta3);
theta4_sol = double(solutions.theta4);
for i = 1:length(theta2_sol)
theta2_val = theta2_sol(i);
theta3_val = theta3_sol(i);
theta4_val = theta4_sol(i);
% 計(jì)算誤差
x_calc = L4 * cosd(apha1) + L3 * cosd(apha1 + theta4_val) + L2 * cosd(theta2_val);
z_calc = L1 + L4 * sind(apha1) + L3 * sind(apha1 + theta4_val) + L2 * sind(theta2_val);
% 計(jì)算誤差
error = sqrt((x_calc - x_target)^2 + (z_calc - z_target)^2);
% 更新最優(yōu)解
if error < min_error
min_error = error;
best_theta = [0, theta2_val, theta3_val, theta4_val, 0, 0];
end
end
end
C_theta1= [0, 0,theta2_val, theta3_val, theta4_val-90, 0, 0];
T_result = eye(4); % 初始化結(jié)果為單位矩陣
for i = 1:6
T = [cosd(C_theta1(i+1)), -sind(C_theta1(i+1)), 0, C_a(i);
sind(C_theta1(i+1))*cosd(C_alpha(i)), cosd(C_theta1(i+1))*cosd(C_alpha(i)), -sind(C_alpha(i)), -sind(C_alpha(i))*C_d(i+1);
sind(C_theta1(i+1))*sind(C_alpha(i)), cosd(C_theta1(i+1))*sind(C_alpha(i)), cosd(C_alpha(i)), cosd(C_alpha(i))*C_d(i+1);
0, 0, 0, 1]; % 根據(jù)給定的公式計(jì)算 T[i]
T_result = T_result * T; % 乘以每個(gè) T[i]
end
%% 輸出結(jié)果
if isempty(best_theta)
fprintf('無法找到滿足條件的解。\n');
else
fprintf('最優(yōu)解：\n');
fprintf('theta1: %.2f\n', best_theta(1));
fprintf('theta2: %.2f\n', best_theta(2));
fprintf('theta3: %.2f\n', best_theta(3));
fprintf('theta4: %.2f\n', best_theta(4));
fprintf('theta5: %.2f\n', best_theta(5));
fprintf('theta6: %.2f\n', best_theta(6));
fprintf('最小誤差: %.2f\n', min_error);
fprintf('x_calc: %.2f\n', x_calc);
fprintf('z_calc: %.2f\n', z_calc);
disp('T_result 矩陣：');
disp(T_result);
disp('T_target 矩陣：');
disp(T_target);
end
成功的代碼，驗(yàn)證了一下效果很好
clc; clear;
% 機(jī)械臂關(guān)節(jié)長(zhǎng)度
l1 = 100;
l2 = 105;
l3 = 98;
l4 = 245;
IN_theta = [0, -45, 20, 90, 0];
% DH 參數(shù)
C_a = [0, 0, l2, l3, 0, 0, 0];
C_d = [0, l1, 0, 0, 0, 0, l4];
C_alpha = [0, -90, 0, 0, -90, 0, 0];
C_theta = [0, IN_theta(1), IN_theta(2), IN_theta(3), IN_theta(4)-90, IN_theta(5), 0];
T_target = eye(4); % 初始化結(jié)果為單位矩陣
for i = 1:6
T = [cosd(C_theta(i+1)) -sind(C_theta(i+1)) 0 C_a(i-1+1);
sind(C_theta(i+1))*cosd(C_alpha(i-1+1)) cosd(C_alpha(i-1+1))*cosd(C_theta(i+1)) -sind(C_alpha(i-1+1)) -sind(C_alpha(i-1+1))*C_d(i+1);
sind(C_theta(i+1))*sind(C_alpha(i-1+1)) cosd(C_theta(i+1))*sind(C_alpha(i-1+1)) cosd(C_alpha(i-1+1)) cosd(C_alpha(i-1+1))*C_d(i+1);
0 0 0 1]; % 根據(jù)給定的公式計(jì)算T[i]
T_target = T_target * T; % 乘以每個(gè)T[i]
end
C_theta1 = calculate_joint_angles(T_target(1, 4), T_target(2, 4), T_target(3, 4));
T_result = eye(4); % 重新初始化結(jié)果為單位矩陣
for i = 1:6
T = [cosd(C_theta1(i+1)) -sind(C_theta1(i+1)) 0 C_a(i-1+1);
sind(C_theta1(i+1))*cosd(C_alpha(i-1+1)) cosd(C_alpha(i-1+1))*cosd(C_theta1(i+1)) -sind(C_alpha(i-1+1)) -sind(C_alpha(i-1+1))*C_d(i+1);
sind(C_theta1(i+1))*sind(C_alpha(i-1+1)) cosd(C_theta1(i+1))*sind(C_alpha(i-1+1)) cosd(C_alpha(i-1+1)) cosd(C_alpha(i-1+1))*C_d(i+1);
0 0 0 1]; % 根據(jù)給定的公式計(jì)算T[i]
T_result = T_result * T; % 乘以每個(gè)T[i]
end
disp('T_target 矩陣：');
disp(T_target);
disp('T_result 矩陣：');
disp(T_result);
function C_theta1 = calculate_joint_angles(x, y, z)
% 機(jī)械臂關(guān)節(jié)長(zhǎng)度
l1 = 100;
l2 = 105;
l3 = 98;
l4 = 245;
% 計(jì)算 theta1
theta1 = atan2(y, x);
% 在 x-y 平面投影中的半徑
r = sqrt(x^2 + y^2);
% 初始化找到解的標(biāo)志
found_solution = false;
% 循環(huán)嘗試不同的 a 值
for a_deg = -90:90
a = deg2rad(a_deg);
% 計(jì)算 xc 和 zc
xc = r - cos(a) * l4;
zc = z - sin(a) * l4;
% 計(jì)算 lac_sq
lac_sq = xc^2 + (zc - l1)^2;
% 檢查是否在范圍內(nèi)
if lac_sq > (l2 + l3)^2 || lac_sq < (l2 - l3)^2
continue;
end
% 計(jì)算 jbac 和 jcac_prime
jbac = acos((l2^2 + lac_sq - l3^2) / (2 * l2 * sqrt(lac_sq)));
jcac_prime = atan2(zc - l1, xc);
% 計(jì)算 theta2
theta2 = -jbac - jcac_prime;
% 計(jì)算 theta3
theta3 = pi - acos((l2^2 + l3^2 - lac_sq) / (2 * l2 * l3));
% 檢查是否有虛數(shù)部分
if isreal(theta2) && isreal(theta3)
% 計(jì)算 theta4
theta4 = - theta2 - theta3 - a;
% 標(biāo)記已找到解
found_solution = true;
break; % 結(jié)束循環(huán)
end
end
if found_solution
% 返回關(guān)節(jié)角度數(shù)組
C_theta1 = [0, rad2deg(theta1), rad2deg(theta2), rad2deg(theta3), rad2deg(theta4)-90, 0, 0];
disp(['Theta1: ', num2str(rad2deg(theta1)), ' degrees']);
disp(['Theta2: ', num2str(rad2deg(theta2)), ' degrees']);
disp(['Theta3: ', num2str(rad2deg(theta3)), ' degrees']);
disp(['Theta4: ', num2str(rad2deg(theta4)), ' degrees']);
else
disp('找不到合適的逆解');
C_theta1 = NaN(1, 7); % 返回 NaN 表示未找到解
end
end
加入插值之后用于機(jī)械臂畫直線的代碼：
clc;clear;
%%根據(jù)起始點(diǎn)和末端點(diǎn)插值
% 定義起始點(diǎn)和終止點(diǎn)
startPoint = [280, 0, 371];
endPoint = [252, 0, 404];
% 定義插值點(diǎn)的數(shù)量
numPoints = 11;
% 使用 linspace 生成插值點(diǎn)
xValues = linspace(startPoint(1), endPoint(1), numPoints);
yValues = linspace(startPoint(2), endPoint(2), numPoints);
zValues = linspace(startPoint(3), endPoint(3), numPoints);
% 存儲(chǔ)插值點(diǎn)的坐標(biāo)
interpolatedPoints = [xValues; yValues; zValues]';
params = [xValues; yValues; zValues];
% 調(diào)用函數(shù)計(jì)算關(guān)節(jié)角度
results = calculate_joint_angles(interpolatedPoints);
function results = calculate_joint_angles(params)
% 初始化角度
theta1 = 0;
theta2 = 0;
theta3 = 0;
theta4 = 0;
% 機(jī)械臂關(guān)節(jié)長(zhǎng)度
l1 = 153;
l2 = 105;
l3 = 98;
l4 = 173;
% 初始化結(jié)果數(shù)組
num_points = size(params, 1);
results = zeros(num_points, 4);
for i = 1:num_points
x = params(i, 1);
y = params(i, 2);
z = params(i, 3);
% 計(jì)算 theta1
theta1 = atan2(y, x);
% 在 x-y 平面投影中的半徑
r = sqrt(x^2 + y^2);
% 初始化找到解的標(biāo)志
found_solution = false;
% 循環(huán)嘗試不同的 a 值
for a_deg = -90:90
a = deg2rad(a_deg);
% 計(jì)算 xc 和 zc
xc = r - cos(a) * l4;
zc = z - sin(a) * l4;
% 計(jì)算 lac_sq
lac_sq = xc^2 + (zc - l1)^2;
% 檢查是否在范圍內(nèi)
if lac_sq > (l2 + l3)^2 || lac_sq < (l2 - l3)^2
continue;
end
% 計(jì)算 jbac 和 jcac_prime
jbac = acos((l2^2 + lac_sq - l3^2) / (2 * l2 * sqrt(lac_sq)));
jcac_prime = atan2(zc - l1, xc);
% 計(jì)算 theta2
theta2 = -jbac - jcac_prime;
% 計(jì)算 theta3
theta3 = pi - acos((l2^2 + l3^2 - lac_sq) / (2 * l2 * l3));
% 檢查是否有虛數(shù)部分
if isreal(theta2) && isreal(theta3)
% 計(jì)算 theta4
theta4 = -a - theta2 - theta3;
% 保存計(jì)算結(jié)果
results(i, :) = [rad2deg(theta1), rad2deg(theta2)+180, rad2deg(theta3)+135, rad2deg(theta4)+135];
%180,135是使機(jī)械臂處于正的初始位置（正水平放置）的舵機(jī)的角度
% 標(biāo)記已找到解
found_solution = true;
break; % 結(jié)束循環(huán)
end
end
% 如果沒有找到解，提示錯(cuò)誤
if ~found_solution
error(['No valid solution found for point ', num2str(i)]);
end
end
% 輸出結(jié)果
disp('Calculated Joint Angles (in degrees):');
disp(results);
end
柚子快報(bào)邀請(qǐng)碼778899分享：5.27機(jī)器人基礎(chǔ)-機(jī)械臂逆解
推薦閱讀
本文內(nèi)容根據(jù)網(wǎng)絡(luò)資料整理，出于傳遞更多信息之目的，不代表金鑰匙跨境贊同其觀點(diǎn)和立場(chǎng)。
轉(zhuǎn)載請(qǐng)注明，如有侵權(quán)，聯(lián)系刪除。
ThisShop出海購物坊
