机器人项目学习记录

首先讲一下项目启动的过程，项目的目录如下图所示：

项目目录.jpg

启动USB7635_DA.vi，启动界面如下，输出范围选择0~10v。

USB7635_DA.jpg
启动USB7660_指定通道采集.vi，界面如下，采集通道为13。
USB7660指定通道采集.jpg

启动main.vi，期望轨迹选择theta_x = 0.2，theta_y = 0.1，theta_z = 0.1，频率fre = 0.05；初始化以后运行vi即可开始机器人的轨迹追踪。

main.jpg
控制的逻辑是：将角度传感器采集到的角度转换为柔索长度，转换函数为：

%% 3自由度脚踝康复机器人机构仿真函数
%l 返回柔索长度矩阵
% theta_x%平台绕x轴旋转角度
% theta_y%平台绕y轴旋转角度
% theta_z%平台绕z轴旋转角度

H=[0 0 -477]';
%% 定平台坐标点
p1f=[-187.5,89,0]'; %p1f=[89,187.5,0]';
p2f=[187.5,89,0]'; %p2f=[89,-187.5,0]';
p3f=[-172,50,-583]'; %p3f=[50,172,-583]';
p4f=[172,50,-583]'; %p4f=[50,-172,-583]';
p5f=[0,-74,-345]'; %p5f=[-74,0,-345]';

%% 动平台坐标点
p1m=[-60,50,-106]'; %p1m=[50,60,-106]';
p2m=[60,50,-106]'; %p2m=[50,-60,-106]';
p3m=[-60,50,-106]'; %p3m=[50,60,-106]';
p4m=[60,50,-106]'; %p4m=[50,-60,-106]';
p5m=[0,-74,-106]'; %p5m=[-74,0,-106]';

%% 旋转矩阵
T_x=[1 0 0;
    0 cos(theta_x) -sin(theta_x);
    0 sin(theta_x) cos(theta_x)];
T_y=[cos(theta_y) 0 sin(theta_y);
    0 1 0;
    -sin(theta_y) 0 cos(theta_y)];
T_z=[cos(theta_z) -sin(theta_z) 0;
    sin(theta_z) cos(theta_z) 0;
    0 0 1];
T=T_z*T_y*T_x;

L(1:3,1)=T*p1m+H-p1f;
L(1:3,2)=T*p2m+H-p2f;
L(1:3,3)=T*p3m+H-p3f;
L(1:3,4)=T*p4m+H-p4f;
L(1:3,5)=T*p5m+H-p5f;

for i=1:5
    l(i)=sqrt(L(1,i)^2+L(2,i)^2+L(3,i)^2);
end
l1=(66-l(1)+598.0520)/1000;%25.1227
l2=(66-l(2)+598.0520)/1000;
l3=(46-l(3)+112)/1000;%22.0110
l4=(46-l(4)+112)/1000;
l5=(40-l(5)+238)/1000;%21.8995

%l1=l(1)-598.0520;%25.1227
%l2=l(2)-598.0520;
%l3=l(3)-112;%22.0110
%l4=l(4)-112;
%l5=l(5)-238;%21.8995

通过三个角度计算出5根气动肌肉的长度。这个长度的计算是采用期望角度计算出来的气动肌肉的期望长度，而实际的柔索长度是通过长度传感器计算出来的。得到实际长度和期望长度以后，计算期望轨迹的一阶导速度，二阶导加速度和实际轨迹的一阶导速度，得到5根肌肉的实际轨迹和期望轨迹以后使用控制算法得到控制律，控制律的算法为：

gama = 0.5;
c1 = 2800;
k1 = 2800;
e1  = x - xd;
de1 = dx - dxd;
alfa1 = c1 * e1;
e2 = dx - dxd + alfa1;

s = k1 * e1 + e2;
F1 = gama * s(1);
F2 = gama * s(2);
F3 = gama * s(3);
F4 = gama * s(4);
F5 = gama * s(5);

Fp=[F1;F2;F3;F4;F5]';
c1 = 3800;
k1 = 3800;
h = 1.5;
beta = 1.5;

e1 = x - xd;
de1 = dx - dxd;
e2 = dx - dxd + c1 * e1;
s = k1 * e1 + e2;
ut = (-k1 * (e2 - c1 * e1) - f  - Fp + ddxd - c1 * de1 - h * (s + beta * sign(s))) ./ b;
p1 = ut(1);
p2 = ut(2);
p3 = ut(3);
p4 = ut(4);
p5 = ut(5)

其中x为期望轨迹的数组，xd为实际轨迹的数组。f，b是通过实际轨迹和外部负载计算出的力和参数b。
计算出来控制律以后，在USB7635_DA来控制气动肌肉的输出。