请你先充分调研labview调用matlab的节点法原理、案例、注意事项等等，然后充分学习后再来处理我的问题。下面这个是我的“重力补偿”源代码，分为参数辨识和重力补偿两部分，现在想用labview调用matlab进行处理。参数辨识我在matlab中手动输入，作为已知参数进行重力补偿。而重力补偿打算用matlab的script节点法进行处理，所以请帮我修改下面我所给的完整代码，以在labview中可以实现“当分开在线输入六个关节角，以及六个力和力矩值时”，可以实现重力补偿。
脚本节点都是real格式，接收LabVIEW输入格式：
q = [q1, q2, q3, q4, q5, q6];
sensor_data = [F_x, F_y, F_z, M_x, M_y, M_z];
tool_mass = tool_mass;
tool_mass_center = [cx, cy, cz];
sensor_zero_point = [sz_Fx, sz_Fy, sz_Fz, sz_Mx, sz_My, sz_Mz];
base_angle = [ba1, ba2];
参数辨识结果如下：
***************************************************************************************************************************
+工具质量： 12.444672 Kg
+工具质心位置（基于传感器坐标系）： (0.987002, -13.626192, 89.808066) mm
+传感器零点： (-13.894974, -3.541428, -0.065894, 0.245015, -0.007747, -0.098537) N,N·M
+基座安装倾角： (0.004430, -0.020849) deg
所有代码如下：
%************************************************ % ER35-1900机器人工具参数辨识与重力补偿整合代码 % 功能：读取Excel数据→参数辨识→重力补偿计算→输出全部结果 %************************************************
%第188行
%% 辅助函数定义：向量转反对称矩阵
%参数辨识的辅助函数1
function S = skewSymmetricMatrix(v)
S = [0, -v(3), v(2);
v(3), 0, -v(1);
-v(2), v(1), 0];
end
%% 辅助函数定义：组合装配体质心计算
function [sub_mass, sub_mass_center] = Assembly_mass_calculate (total_mass, total_mass_center, sub_mass_set, sub_mass_center_set)
% 检查输入参数数量
if nargin < 4 || isempty (sub_mass_set) || isempty (sub_mass_center_set)
error ("缺少必要参数：sub_mass_set 或 sub_mass_center_set");
end
[~, num_sub_mass_center_set] = size (sub_mass_center_set);
num_sub_mass_set = length (sub_mass_set);
if num_sub_mass_set ~= num_sub_mass_center_set
error ("输入参数数量不匹配");
end
sub_mass = total_mass - sum(sub_mass_set);
sum_sub_mass_x = 0;
sum_sub_mass_y = 0;
sum_sub_mass_z = 0;
for i = 1:num_sub_mass_set
sum_sub_mass_x = sum_sub_mass_x + sub_mass_set(i)*sub_mass_center_set(1,i);
sum_sub_mass_y = sum_sub_mass_y + sub_mass_set(i)*sub_mass_center_set(2,i);
sum_sub_mass_z = sum_sub_mass_z + sub_mass_set(i)*sub_mass_center_set(3,i);
end
sub_mass_center(1) = (total_masstotal_mass_center(1) - sum_sub_mass_x)/sub_mass;
sub_mass_center(2) = (total_masstotal_mass_center(2) - sum_sub_mass_y)/sub_mass;
sub_mass_center(3) = (total_mass*total_mass_center(3) - sum_sub_mass_z)/sub_mass;
end
%% 机器人模型定义函数
function ER35_1900 = ER35_1900_force_D_H_moxing ()
% ER35-1900 机器人 DH 参数模型定义
angle=pi/180;
% 输入连杆参数
%D-H参数表
a=[0.1486,0.8313,0.095,0,0,0];
d=[0.6696,0,-0.0003,0.8900,0.0002,0.1915];
alp=[-1.5704,0.0003,-1.5709,1.5711,-1.5708,0];
%建立各个连杆
L(1) = Link('d', d(1), 'a', a(1), 'alpha', alp(1),'standard');
L(2) = Link('d', d(2), 'a', a(2), 'alpha', alp(2),'standard');
L(3) = Link('d', d(3), 'a', a(3), 'alpha', alp(3),'standard');
L(4) = Link('d', d(4), 'a', a(4), 'alpha', alp(4),'standard');
L(5) = Link('d', d(5), 'a', a(5), 'alpha', alp(5),'standard');
L(6) = Link('d', d(6), 'a', a(6), 'alpha', alp(6),'standard');
% L2.offset = pi/2的意思为第二根轴相对于第一根轴有90度的关节偏移量，也就是初始状态下连杆2对于连杆1来说有一个关节的偏置。
% 建立串联机械臂
ER35_1900=SerialLink([L(1) L(2) L(3) L(4) L(5) L(6)],'name','ER35_1900@MOD');
end
%% TCP 受力计算函数
function Fe = ER35_1900_force_tcp_force_calculate(Rbs, Tse, sensor_data, sensor_zero_point, tool_mass, tool_mass_center, base_angle)
Rwb = rotx(base_angle(1))*roty(base_angle(2)); % 基坐标系{B}相对世界坐标系{W}的旋转矩阵
Rsc = Rbs'*Rwb'; % 工具质心坐标系{C}相对传感器标定参考系{S}的旋转矩阵，倾角传感器输入
Tf1 = [Rsc, zeros(3); skewSymmetricMatrix(tool_mass_center)*Rsc, Rsc]; % 工具重力从工具质心到传感器标定参考系的转换
Fs_G = Tf1 * [0, 0, -tool_mass*9.8011, 0, 0, 0]'; % 工具重力对传感器标定参考系{S}的力和力矩
Fs_e = sensor_data - sensor_zero_point - Fs_G; % 工具tcp所受外力和外力矩对传感器标定参考系{S}的力和力矩
Rse = Tse(1:3,1:3); % tcp点相对于传感器标定参考系{S}的旋转矩阵
Pse = Tse(1:3,4); % tcp点在传感器标定参考系{S}的坐标系
Tf2 = [Rse, zeros(3); skewSymmetricMatrix(Pse)*Rse, Rse]; % 工具tcp所受外力和外力矩从tcp点到传感器标定参考系的转换
Fe = inv(Tf2)*Fs_e; % 工具tcp点处所受的外力和外力矩
end
%% 工具参数辨识函数
function [tool_mass, tool_mass_center, sensor_zero_point, base_angle] = ER35_1900_force_parameter_indenfication(pose_set, measure_set, ext_mass_set, ext_mass_center_set)
% Parse input parameters 检查输入参数数目
narginchk(2, 4); % check the number of input arguments
% 检查 ext_mass_set、ext_mass_center_set，是否考虑额外质量
var_exist_flag = exist('ext_mass_set', 'var') | exist('ext_mass_center_set', 'var');
if(var_exist_flag)
cal_ext_flag = true;
else
cal_ext_flag = false;
end
%释义：检查当前函数调用时输入参数的数量是否在 2 到 4 个之间；然后检查工作区中 ext_mass_set 或 ext_mass_center_set 变量是否存在；最后根据变量存在情况为 cal_ext_flag 赋值，若至少有一个变量存在，cal_ext_flag 为 true，否则为 false。
I = eye(3); %3*3的单位矩阵
A = [];
B = [];
f = [];
n = [];
for i = 1:length(pose_set)
A = [A;pose_set(:,:,i)', I]; %将机器人的RS_B求逆，与I组成新矩阵，再与原来的组成新新矩阵，可看零点力计算推导
B = [B;-skewSymmetricMatrix(measure_set(1:3,i)), I]; %将传感器力值的求反对称矩阵，与I组成新矩阵，再与原来的组成新新矩阵，可看零点力矩计算推导
f = [f,measure_set(1:3,i)']; %读取的是传感器的力值
n = [n,measure_set(4:6,i)']; %读取的是传感器的力矩值
end
f = f'; n = n';
% 最小二乘求解
x1 = inv (A'*A)*A'*f;
x2 = inv (B'*B)*B'*n;
% 提取辨识结果
% 工具重力
tool_mass = norm(x1(1:3));
% 传感器零点力、力矩
sensor_zero_point = [x1(4:6)', transpose(x2(4:6)-skewSymmetricMatrix(x1(4:6))*x2(1:3))]';
% 工具质心
tool_mass_center = x2(1:3);
% 基座安装倾角
base_angle = [asin(-x1(2)/tool_mass), atan(-x1(1)/x1(3))];
% 附加质量补偿（增加参数检查）
if cal_ext_flag
if ~exist ('ext_mass_set', 'var') || isempty (ext_mass_set) || ...
~exist ('ext_mass_center_set', 'var') || isempty (ext_mass_center_set)
warning (' 可选参数 ext_mass_set 或 ext_mass_center_set 为空，跳过补偿计算 ');
else
[tool_mass, tool_mass_center] = Assembly_mass_calculate (tool_mass, tool_mass_center, ext_mass_set, ext_mass_center_set);
end
end
end
%% ============== 主程序：参数辨识与重力补偿全流程 ==============
fprintf ("开始执行 ER35-1900 机器人工具参数辨识与重力补偿计算...\n");
%% 1. 加载数据（参数辨识所需数据）
try
data_identify = readmatrix ("ER35_1900_M3815E1.xlsx", 'Range', 'B3:S8');
fprintf ("成功加载参数辨识数据，数据范围：B4:S8\n");
catch ME
error ("参数辨识数据加载失败，请检查文件路径和格式：% s", ME.message);
end
%% 2. 工具参数辨识
fprintf ("\n------------------- 开始工具参数辨识 -------------------\n");
% 加载机器人模型
ER35_1900 = ER35_1900_force_D_H_moxing ();
fprintf ("机器人模型加载完成 \n");
% 准备姿态数据
q0 = deg2rad (data_identify (:, 1:6));
num_rows = size (data_identify, 1);
pose_set = zeros (3, 3, num_rows);
for i = 1:num_rows
% 关节角度变换
q = [q0(i,1)+3.289353991486221e-04 -q0(i,2)-(pi/2)+2.099233065083616e-04 -q0(i,3)+6.461884942503554e-04 q0(i,4)+0.001523703947477 -q0(i,5)+2.022937118746407e-04 q0(i,6)+pi/2];
T = ER35_1900.fkine (q);
if i == 1
disp ("第一个姿态的齐次变换矩阵 T："); disp (T);
disp ("T 的类型："); disp (class (T));
end
% 提取旋转矩阵
pose_set (:,:,i) = T (1:3, 1:3);
end
% 准备力传感器数据
measure_set = data_identify (:,13:18)';
fprintf ("成功提取 % d 组力传感器数据 \n", num_rows);
% 执行参数辨识（不带附加质量）
[tool_mass, tool_mass_center, sensor_zero_point, base_angle] = ER35_1900_force_parameter_indenfication (pose_set, measure_set);
% 输出辨识结果（转换为实际单位）
fprintf("\n参数辨识结果如下：\n");
fprintf("***************************************************************************************************************************\n")
fprintf("+工具质量：\t\t\t\t%f Kg\n", tool_mass/9.8011);
fprintf("+工具质心位置（基于传感器坐标系）：\t(%f, %f, %f) mm\n", tool_mass_center*1000);
fprintf("+传感器零点：\t\t\t\t(%f, %f, %f, %f, %f, %f) N,N·M\n", sensor_zero_point);
fprintf("+基座安装倾角：\t\t\t(%f, %f) deg\n", base_angle);
fprintf("***************************************************************************************************************************\n")
%% 3. 重力补偿计算
fprintf ("\n------------------- 开始 TCP 重力补偿计算 -------------------\n");
try
% 加载重力补偿所需数据（假设数据范围不同）
data_compensate = readmatrix ("ER35_1900_M3815E1.xlsx", 'Range', 'B3:S15');
fprintf ("成功加载重力补偿数据，数据范围：B3:S15\n");
catch ME
error ("重力补偿数据加载失败，请检查文件路径和格式：% s", ME.message);
end
% 准备姿态数据
% prepare robot pose data
q0=deg2rad(data_compensate(:, 1:6)); %输入各关节角
% 获取数据行数
num_poses = size(data_compensate, 1)
% 对每一行数据单独处理
for i = 1:num_poses
% 对当前行进行关节角度变换
q = [q0(i,1)+3.289353991486221e-04 -q0(i,2)-(pi/2)+2.099233065083616e-04 -q0(i,3)+6.461884942503554e-04 q0(i,4)+0.001523703947477 -q0(i,5)+2.022937118746407e-04 q0(i,6)+pi/2];
% 计算当前行的齐次变换矩阵
T = ER35_1900.fkine(q);
% 仅在第一行时显示T的内容和类型用于调试
if i == 1
disp(T);
disp(class(T));
end
% 提取旋转矩阵部分存入pose_set
pose_set_compensate(:,:,i) = T(1:3, 1:3);
end
% prepare sensor force data
measure_set_compensate = data_compensate(:,13:18)';
% 选取第 i 组数据计算（可修改 i 值选择不同组）
i = 5;
if i > num_poses
i = num_poses;
warning ("选取的组数超过数据范围，自动使用最后一组数据");
end
Rbs = pose_set_compensate (:,:,i);
Tse = eye (4); % 假设工具坐标系无偏移
sensor_data = measure_set_compensate (:,i);
% 执行 TCP 受力计算
Fe = ER35_1900_force_tcp_force_calculate (Rbs, Tse, sensor_data, sensor_zero_point, tool_mass/9.8011, tool_mass_center, base_angle);
% 输出重力补偿结果
fprintf ("\n------------------- TCP 重力补偿计算结果 -------------------\n");
fprintf ("第 % d 组数据计算结果：\n", i);
fprintf ("经过重力补偿后 TCP 所受外力为：(% f, % f, % f, % f, % f, % f) N/ N・M\n", Fe);
fprintf ("***************************************************************************************************************************\n");
fprintf ("计算完成！\n");
错误1050发生于 重力补偿版本V1.0.vi中的LabVIEW: (Hex 0x441) 执行脚本时出错。来自服务器的错误消息：??? 错误: 函数定义在此上下文中不受支持。函数只能作为代码文件中的局部函数或嵌套函数创建。
。
本文提供了解决Android应用中图片加载失败及程序无法运行的问题的方法，包括重启模拟器、检查图片资源路径、优化代码实现等步骤。
扫一扫
984
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
