问题

做实验过程中使用 EM tracker 测量自己机器人末端旋转时的角度。
尾部 设置EMTracker 1，作为固定基准，其轴线与机器人中心轴线近似重合，EM Tracker 2 固定在机器人活动关节上，两者轴线夹角近似为机器人旋转角度。论文尚未发表，图片仅展示部分。

实验结果

实验数据中存在的问题

1. 上述方式存在安装误差，图中可以看出，初始误差有接近10 度
2. 角度只有正值，并且不能展示左右两侧，峰值小一点的应该是弯去另一侧，其值应该为负数。
3. 在 6度 多的时候存在换向，换向附近，角度斜率明显降低

解决办法

向量夹角，最低点处翻转

t_0 = 32;							% 第一个周期的开始时间
Period=90.5-31.2;			% 周期
t_1 = t_0; 
t_2 = t_0;
while true
    t_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 degree

    
for i = 1:size(Vect_1,2)
    if size(Vect_2,2)==1
        V_2 = Vect_2;
    else
        V_2 = Vect_2(:,i);
    end
    V_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%2FProjectionOfVectorOntoPlane

if nargin == 1
    Vect_2 = [1; 0; 0;];
end

for i = 1:size(Vect_1,2)
    vector2plane(:, i) = Vect_1(:,i)- dot(Vect_1(:,i), Vect_2)/norm(Vect_2) * Vect_2;
end


end

结果

附录

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;
else
    disp('----------------------------------');
    disp('Variable Name Error!')
    disp('----------------------------------');
end

time_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