问题
做实验过程中使用 EM tracker 测量自己机器人末端旋转时的角度。
尾部 设置EMTracker 1，作为固定基准，其轴线与机器人中心轴线近似重合，EM Tracker 2 固定在机器人活动关节上，两者轴线夹角近似为机器人旋转角度。论文尚未发表，图片仅展示部分。
实验结果
实验数据中存在的问题
- 上述方式存在安装误差，图中可以看出，初始误差有接近10 度
- 角度只有正值，并且不能展示左右两侧，峰值小一点的应该是弯去另一侧，其值应该为负数。
- 在 6度 多的时候存在换向，换向附近，角度斜率明显降低
解决办法
向量夹角，最低点处翻转
t_0 = 32; % 第一个周期的开始时间
Period=90.5-31.2; % 周期
t_1 = t_0;
t_2 = t_0;
while truet_1 = t_2;t_2 = t_1 + Period;t_tmp= t_1 + Period*0.5;if t_2 > t_max_min % 还在实验时间内break;end
idx_1 = round(t_1/0.05)+1; % 计算时间对应的 index 索引
idx_2 = round(t_2/0.05)+1;
idx_tmp= round(t_tmp/0.05)+1; % 分割周期中心，分别求两个拐点
[M_1,I_1] = min(Ang_EM2inEM20(idx_1:idx_tmp)); % 前半个周期的最小值，是第一个拐点
[M_2,I_2] = min(Ang_EM2inEM20(idx_tmp:idx_2));idx_start = idx_1 + I_1 - 1; % 根据最小值的 index 还原到整个数据序列的索引
idx_stop = idx_tmp + I_2 - 1;% Ang_EM2inEM20(idx_start:idx_stop) = M_1 + M_2 -Ang_EM2inEM20(idx_start:idx_stop);
% Ang_EM2inEM1(idx_start:idx_stop) = M_1 + M_2 -Ang_EM2inEM1(idx_start:idx_stop);end
不使用两个向量夹角表示旋转角
安装过程中，两个 EM tracker 不可能在同一条轴线 或者同一个平面运动，因此，前端 EM tracker 2 随机器人运动过程中，其轴线与末端固定的 EM Tracker 1 一直是空间角。空间角基本不存在夹角为 0 的时候。这是为什么实验数据中最小值在 6 度就拐弯了。另外空间角对时间的变化率在接近其最小值的时候会变化，这是为什么接近最小值时，明显有弧度拐弯的原因。
改进方法： 采用 EM tracker 2 与其初始未运动时向量夹角表示机器人旋转角度。
Ang_EM2inEM20 = VectorAngle(z2_in_YOZ, mean(z2_in_YOZ(:, 5:100), 2));
function vect_ang = VectorAngle(Vect_1, Vect_2)
% This function is to calculate the angle between two axes
% Input
% Vect_1 ----- the variable vector, column vector
% Vect_2 ----- reference vector, column vectors, or a unchangable
% ----- column vector
% Output
% vect_ang ----- Angle series in degreefor i = 1:size(Vect_1,2)if size(Vect_2,2)==1V_2 = Vect_2;elseV_2 = Vect_2(:,i);endV_1 = Vect_1(:,i);vect_ang(i) = rad2deg( atan2(norm(cross(V_2', V_1')), dot(V_2, V_1)) );
end% theta = subspace(Vect_1,Vect_2);
disp('')end
解决空间角变化速率的问题
上一节分析中知道 空间角变化了 会导致拐角处圆角。由于机器人运动轴线是 x 轴，机器人实际在 YOZ 平面运动，因此讲 EM Tracker 2 的轴向向量投影到 EM tracker base frame 的 YOZ 平面在计算夹角。
[z2_series, ~] = Quat2zaxis(EM_mat_2); % 原始 四元数转 z 轴
z2_in_YOZ = VectorProjection(z2_series);
Ang_EM2inEM20 = VectorAngle(z2_in_YOZ, mean(z2_in_YOZ(:, 5:100), 2));
function vector2plane = VectorProjection(Vect_1, Vect_2)
% This function is to calculate the projection of Vect_1 in a plane (with
% norm vector Vect_2)
% Input
% Vect_1 ----- the variable vector, column vector
% Vect_2 ----- Normal vector of the projection plane, default YOZ
% plane of EM tracker, [1 0 0]
% Output
% vector_plane----- the projection vector of vect_1 in plane of normal
% vector vect_2
% reference ----- https://www.maplesoft.com/support/help/maple/view.aspx?path=MathApps%2FProjectionOfVectorOntoPlaneif nargin == 1Vect_2 = [1; 0; 0;];
endfor i = 1:size(Vect_1,2)vector2plane(:, i) = Vect_1(:,i)- dot(Vect_1(:,i), Vect_2)/norm(Vect_2) * Vect_2;
endend结果
附录
function [z_axis_series, time_series] = Quat2zaxis(EM_mat)
% This function is to extract the z-axis of EM tracker in EM base frame
% Input
% EM_mat ----- .mat file of the EM tracker
% Output
% z_in_base ----- each row is the z-axis coordinate in EM base frame
% time_series ----- time series, load(EM_mat);
disp('');
varlist = who;
TF = contains(varlist,'EM_tracker');
EM_Name = varlist(TF);
if contains(EM_Name, '_1')EM_Data = PosAndOri_EM_tracker_1;
elseif contains(EM_Name, '_2')EM_Data = PosAndOri_EM_tracker_2;
elseif contains(EM_Name, '_3')EM_Data = PosAndOri_EM_tracker_3;
elseif contains(EM_Name, '_4')EM_Data = PosAndOri_EM_tracker_4;
elsedisp('----------------------------------');disp('Variable Name Error!')disp('----------------------------------');
endtime_series = EM_Data(1,:);
quat_data = EM_Data(5:8,:);
z_axis_series = [];for i = 1:size(quat_data,2)rotm = quat2rotm(transpose( quat_data(:,i)) );z_axis_series(1:3, i) = rotm(:,3);
end% z_in_base(1,:) = time_series;
% z_in_base(2:4,:) = z_axis_series;end
