我们的目标是创建一个四足步行机器人。首先，我们创建一个三轴机器臂,将其作为步行机器人的一条腿，并为其规划一个适合步行的轨迹，然后将四条腿组合以完成最后的步行机器人。
1 创建一条机器人腿
首先要确定我们的坐标系。如上图所示是机器人腿在零角度位姿时的坐标系设置。我们选择使用航空坐标体系，其 x x x轴指向前, z z z轴指向下,因而 y y y轴就指向右侧。
腿的第一个关节将实现髋关节的前后摆动，它绕 z z z轴旋转，或( R z ( q 1 ) R_{z}(q_1) Rz(q1))。第二关节是实现髋关节的上下摆动,它绕x轴旋转( R x ( q 1 ) R_{x}(q_1) Rx(q1))。因为上述两个旋转轴相交，它们就形成了一个球髋关节。
膝盖位于沿着 y y y轴方向平移 L L L的位置，或 T y ( L 1 ) T_y(L_1) Ty(L1)。第三个关节实现靠近和远离机体的膝关节运动，即 R x ( q 3 ) R_x(q_3) Rx(q3)。脚尖位于沿着 z z z轴方向平移 L L L的位置，或 T z ( L 2 ) T_z(L_2) Tz(L2)。因此该机器人从髋部到脚尖的变换顺序为 R z ( q 1 ) R x ( q 2 ) T y ( L 1 ) T z ( L 2 ) R_z(q_1)R_x(q_2)T_y(L_1)T_z(L_2) Rz(q1)Rx(q2)Ty(L1)Tz(L2)
R
z
(
q
1
)
R
x
(
q
2
)
R_z(q_1)R_x(q_2)
Rz(q1)Rx(q2)使得
y
y
y沿杠方向。
我们用工具箱确定D-H参数：
s='Rz(q1) Rx(q2) Ty(L1) Rx(q3) Tz(L2)';
dh=DHFactor(s);
dh.display
工具变换：
dh.tool
它改变了脚部坐标系的方向。然而，在这个问题中脚与地面只是一个简单的点接触,所以我们并不关注它的方向。方法dh .command生成一个工具箱命令字符串，用以创建一个 serialLink对象:
dh.command('leg')
结果：
SerialLink([0, 0, 0, pi/2, 0; 0, 0, L1, 0, 0; 0, 0, -L2, 0, 0; ], 'name', 'leg', 'base', eye(4,4), 'tool', trotz(pi/2)*trotx(-pi/2)*trotz(-pi/2), 'offset', [pi/2 0 -pi/2 ])
在MATLAB工作区里将腿的各个部分长度都设置为100毫米之后，将SerialLink对象输入MATLAB的eval命令中：
L1=0.1;L2=0.1;
leg=eval(dh.command('leg'))
在零关节角度时，脚尖处于我们所设计的位置上：
transl(leg.fkine([0,0,0]))
我们可以绘制出机器人的零角度位姿：
transl(leg.fkine([0,0,0]))
leg.plot([0,0,0],'nobase','noshadow')
set(gca,'Zdir','reverse');view(137,48);
现在，我们测试各个关节是否能产生期望的运动：
transl(leg.fkine([0.2,0,0]))'
transl(leg.fkine([0,0.2,0]))'
transl(leg.fkine([0,0,0.2]))'
至此，我们已经创建并验证了一个简单的机器人腿。
2 单腿运动
下一步是定义的腿的末端执行器(即它的脚)将运行的路径。首先考虑的是:所有的脚都在地面上以相同的速度向后移动，其作用是在脚实际上没有相对地面滑动的情况下推动机器人身体向前运动。由于每条腿的运动范围有限，因此它不能后退很长的距离。在某个点我们必须复位机器人腿——抬起脚,向前移动，再放在地面上。第二个考虑是静态稳定性—任何时候机器人必须至少有三只脚同时站在地面上，所以每条腿必须按顺序依次复位。这就要求在一个周期内每条腿有3/4的时间与地面接触，而另外1/4的时间在复位。其结果是,腿在复位期间必须加快移动,因为它要用相对少的时间来完成一段更长的路径。
所需的轨迹是由所有途径点所决定：
xf=50;xb=-xf;y=50;zu=20;zd=50;
path=[xf,y,zd;xb,y,zd;xb,y,zu;xf,y,zu;xf,y,zd]&1e-3;
其中，xf和xb分别是
x
x
x方向上腿部前进和后退运动的极限(以毫米为单位),
y
y
y是
y
y
y方向上脚到机体的距离，而zu和zd分别是在
z
z
z方向上脚抬起和放下时脚的高度。在这个示例中，脚是从髋部前面50毫米运动至后面50毫米。当脚部放下时，它位于髋部下方50毫米，而在复位期间脚提升到髋部下方20毫米处（注意：仿真中z轴设置为向下）。
在path中的点包含一个完整的周期，对应于站立的开始阶段、站立的结束阶段、脚尖抬起阶段、脚尖返回阶段、最后回到站立的开始，如图所示。
% 100HZ的频率对多段路径进行采样
path=mstraj(path,[],[0,3,0.25,0.5,0.25]',path(1,:),0.01,0);
plot3(path(:,1),path(:,2),path(:,3),'color','k','LineWidth',2)
此例中，我们已经指定了一个包含各段运动时间的向量，而不是最大的关节速度。最后3个参数分别是初始位置、采样间隔和加速时间。这个轨迹总的运行时间为4秒，因此它包含了400个点。
我们用逆运动学来确定脚尖路径所需的关节角度轨迹。这个机器人是欠驱动的，所以我们使用广义逆运动学ikine，并通过设置掩盖向量以求解末端执行器的平移：
qcycle=leg.ikine(transl(path),[],[1,1,1,0,0,0]);
% 动画
view(-pi,-pi/2)
leg.plot(qcycle,'loop')
以验证它是否如我们所愿:先沿着地面缓慢运动，然后快速提升、向前运动和放下脚。' loop '选项将使轨迹的显示一直循环不断，而你需要键人Control-C来停止它。
3 四腿运动
定义机器人的宽度和长度：
W=0.1;L=0.2;
通过复制先前生成的leg对象，就可以创建多条腿的机器人。但其中我们要提供不同的基座转换，以将腿连接到机体上不同的点：
legs(1) = SerialLink(leg, 'name', 'leg1');
legs(2) = SerialLink(leg, 'name', 'leg2', 'base', transl(-L, 0, 0));
legs(3) = SerialLink(leg, 'name', 'leg3', 'base', transl(-L, -W, 0)*trotz(pi));
legs(4) = SerialLink(leg, 'name', 'leg4', 'base', transl(0, -W, 0)*trotz(pi));
得到的结果是一个包含多个SerialLink对象的向量。请注意，机体左侧的第3条和第4条腿已经绕着z轴进行了旋转trotz(pi)，使得它们都朝向身体外侧。
如前所述，每条腿必须按照各自的顺序进行复位。由于腿的轨迹是一个周期运动,我们可以通过使每个腿的运动轨迹有一个相位偏移来实现按顺序复位，该相移时间为总循环周期的1/4。由于总周期有400点，因此每条腿的相移为100个点。然后我们使用模运算来模拟每条腿的循环步态。
行走程序的核心是
k=1;
for i=1:500
legs(1).animate( gait(qcycle, k, 0, 0));
legs(2).animate( gait(qcycle, k, 100, 0));
legs(3).animate( gait(qcycle, k, 200, 1));
legs(4).animate( gait(qcycle, k, 300, 1));
drawnow
k = k+1;
end
其中的函数
gait(q，k, ph,. flip)
将通过模运算返回q的第k +ph个元素，而模运算将q看作一个周期。参数flip是将机器人左侧两条腿上关节1运动的符号取反(从动态图可以看到左右第一个关节的转向是相反的)。
clc;
clear
close all
s='Rz(q1) Rx(q2) Ty(L1) Rx(q3) Tz(L2)';
dh=DHFactor(s);
L1=0.1;L2=0.1;
leg=eval(dh.command('leg;'));
% 定义沿着x轴方向走的关键参数
xf = 5; xb = -xf; % x方向上腿前进和后退运动的极限
y = 5; % y方向上脚到机体的距离 运动时是固定的
zu = 2; zd = 5; % z方向上脚抬起和放下时脚的高度。
% 定义方形轨迹
segments = [xf y zd; xb y zd; xb y zu; xf y zu] * 0.01;
segments = [segments; segments]; % 长度为8，两个运动周期分八段
% build the gait. the points are:
% 1 start of walking stroke
% 2 end of walking stroke
% 3 end of foot raise
% 4 foot raised and forward
%
% The segments times are :
% 1->2 3s
% 2->3 0.25s
% 3->4 0.5s
% 4->1 0.25s
tseg = [3 0.25 0.5 0.25]';
tseg = [1; tseg; tseg]; % 在开头增加一个1是为预加速
x = mstraj(segments, [], tseg, segments(1,:), 0.01, 0.1); % 采样间隔为0.01，总的有400step
% 总的为1, 2, 3, 4, 1, 2, 3, 4，去掉加速部分（舍弃一些值），
% 其中的 2->3->4->1 is smooth cyclic motion so we "cut it out" and use it.
xcycle = x(100:500,:);
%=========================================================
% 运行以下程序即可知道"cut it out"的目的。
% figure(1)
% plot3(x(:,1),x(:,2),x(:,3))
% rotate3d on
% figure(2)
% plot3(xcycle(:,1),xcycle(:,2),xcycle(:,3),'color','r')
% rotate3d on
%=========================================================
qcycle = leg.ikine( transl(xcycle), [], [1 1 1 0 0 0], 'pinv' );
% 机器人长方形身体的宽和长
W = 0.1; L = 0.2;
% 创建4条腿的机器人。每条腿都是我们上面造的腿机器人的克隆，
% 通过'base'基变换将四条腿安装在身体的四个角
legs(1) = SerialLink(leg, 'name', 'leg1');
legs(2) = SerialLink(leg, 'name', 'leg2', 'base', transl(-L, 0, 0));
legs(3) = SerialLink(leg, 'name', 'leg3', 'base', transl(-L, -W, 0)*trotz(pi));
legs(4) = SerialLink(leg, 'name', 'leg4', 'base', transl(0, -W, 0)*trotz(pi));
% create a fixed size axis for the robot, and set z positive downward
clf; axis([-0.3 0.1 -0.2 0.2 -0.15 0.05]); set(gca,'Zdir', 'reverse')
hold on
% draw the robot's body
patch([0 -L -L 0], [0 0 -W -W], [0 0 0 0], ...
'FaceColor', 'r', 'FaceAlpha', 0.5);
% 初始化机器人
% 一点优化。我们使用了很多打印选项来快速制作动画：
% 关闭注释，如手腕轴、地面阴影、关节轴、无平滑着色。
% 我们不是在每个循环中解析开关，而是在这里预先将它们分解成plotopt结构。
plotopt = {'noraise', 'nobase', 'noshadow', 'nowrist', 'nojaxes', 'delay', 0};
for i=1:4
legs(i).plot(qcycle(1,:), plotopt{:});
end
hold off
k = 1;
while 1
legs(1).animate( gait(qcycle, k, 0, 0))
legs(2).animate( gait(qcycle, k, 100, 0))
legs(3).animate( gait(qcycle, k, 200, 1))
legs(4).animate( gait(qcycle, k, 300, 1))
drawnow
k = k+1;
end
