基于“白泽”四足机器人足端轨迹的插值方法
目录
1 应用背景
2 模型建立
3 MATLAB对足端轨迹进行拟合插值
4 结果与分析
5 matlab逆运动学程序
1 应用背景
近年来，机器人领域得到了大力的发展，多技术融合赋予了机器人更多的使命，其中足式机器人凭借其在探险、军事等领域的优越运动表现而快速的进入了人们的视野。感知系统（例如视觉识别，超声波传感测距仪以及红外设备等） 的加入使足式机器人具备独立户外工作的能力，它们在行进过程中可以识别出大型障碍物并规划新的路径进行避障，但是很多小型障碍物却无法被它准确的侦测到，这就要求足式机器人能够稳定踏上或者跨越这些不易察觉的小型障碍物，具有最基本的跨越小型障碍物的能力.足端轨迹规划的优劣对机器人跨越障碍方面有重要的影响，使用何种方式规划足端轨迹，足端轨迹闭环曲线线形的控制方式等就成为了研究足式机器人的基础内容。在这里，我们通过三次样条插值的方式，使得机器人足端轨迹的规划足够任意，能够走出各种预期轨迹。
该机器人是我从去年开始开发并维护的一款arduino开源四足机器人，这款机器人将会得到长期的维护和改进，后期会基于ros增加更丰富的功能。
2 模型建立
首先，需要先建立solidworks模型，机器人腿部共有8个自由度，单腿有两个自由度，头部可进行自定义扩展。主控板是一款基于国产lgt8f328p芯片的arduino nano开发板，由一节4.8V可充电电池供电。下图为建好的solidworks模型：
图2-1 机器人solidworks模型
然后，我们可以根据机器人的3D模型，建立DH参数表，通过DH参数模型推导机器人的正逆运动学方程。不过，在得到DH参数表之前，我们先去将模型的坐标系建立好，然后导出urdf模型。
图2-2 Solidworks建立坐标系导出urdf模型
导出urdf模型后，将其加载进入matlab观察坐标系之间的关系：
图2-3 urdf模型导入matlab
上图为加载进matlab的urdf模型，这个是整个机器人的坐标系，而分析四足机器人，我们只要进行单腿运动学分析即可。
因为四足机器人的运动学分析以及运动方程的建立主要基于单腿运动学。因此，我们只需先对单腿进行DH建模并在matlab中进行轨迹插值即可，通过步态相位差可以很方便的将单腿的运动转换到其他腿上达到控制机器人的目的。
单腿的DH参数如下：
表2-1 机器人单腿DH参数表
对机器人进行单腿DH建模（标准DH参数）并进行运动学分析：
图2-4 matlab里对机器人单腿DH建模程序
图2-5 matlab里对机器人单腿DH建模
3 MATLAB对足端轨迹进行拟合插值
为了控制机器人足端能够任意走出我们想要的轨迹，需要通过事先找好轨迹控制点，然后通过插值的方式，插值得到轨迹上其他点的坐标。这样只要我们改变控制点，就可以足够随意的控制足端轨迹，以得到多组机器人足端运动轨迹，通过机器人逆运动学得到每种足端轨迹对应的机器人运动关节变量数据，然后测试每组关节变量数据在不同地形上的行走性能，通过性能的对比，得出在各种不同地形下，机器人最优足端轨迹。
Matlab单腿DH模型取点：
图3-1 matlab里对足端取点
我们在如下图中，构建一组足端轨迹控制点，用这一组点来控制足端轨迹的轮廓，如下图蓝色小圆圈所示为构建的点：
图3-2 完成一组足端轨迹控制点的获取
然后，通过三次样条插值的方式，得到整条轨迹和一组坐标数据，插值梯度可以任意定义，如下图所示：
通过这组插值之后得到的点坐标数据，对机器人单腿用逆运动学计算关节变量，如下图：
图3-5 插值后单腿足端平滑轨迹
通过逆运动学得来的关节变量，再对机器人单腿进行运动学正解，将关节变量赋值给各个关节，并实时展现其过程，观察机器人运动的足端轨迹是否符合插值结果：
通过逆解得来的数据进行正解展示并验证插值结果，四足机器人单腿运动4个时刻的步态帧如下图所示，可以看到每一帧图片中，足端坐标和我们插值结果完美重合，机器人足端走出了预期的轨迹：
4 结果与分析
在基于ubuntu系统并安装了ROS的虚拟机上，编写ROS节点，如下图所示：
若行走步态与预期一致且表现稳定，我们可以将机器人放下来进行步行测试，如下图为在实验室地板进行步行测试：
图4-5 实时步行测试
结果与分析：
通过步行测试发现，足端可以走出我们想要的轨迹，可见matlab插值在四足机器人轨迹控制与规划方面还是非常方便的。通过事先规划好轨迹控制点，通过一组点控制好机器人足端的轨迹轮廓，然后用插值的方式丰富轨迹上点的数量，这样我们能够控制机器人足端尽可能契合预期运动轨迹，同时减少舵机抖舵对机身平衡的影响。
在机器人前进过程中通过对机身上IMU信号的采集，可以分析出这种足端轨迹的优劣，以最终得到满意的足端轨迹。这充分显示了matlab对于缩短开发机器人项目的周期，加快产品开发上的优越性。
5 matlab逆运动学程序
clear;
clc;
theta_0=[];
% 勾画机器人腿部关节
q1_lim = [-pi/2,0];
L1 = Link('d', 0, 'a',45, 'alpha', 0,'offset',0);
L2 = Link('d',0 , 'a', 45 , 'alpha', 0,'offset',0);
quadruped_robot=SerialLink([L1 L2],'name','quadruped_robot_liuzhitong');
quadruped_robot.plot([0 0]);
L1.qlim = q1_lim;
% T4=quadruped_robot.fkine([pi/3,pi/5,pi/7,pi/4]);
axis equal;
%机器人足端轨迹控制点
teach(quadruped_robot);
view([0,0,1]);%XY平面的投影看
hold on;
x = [0,-6.630,-20.091,-40.761];
y = [-57.368,-52.484,-46.427,-56.102];
x1=[-40.761,0];
y1=[-56.102,-57.368];
xx=0:-4:-40.761;
xx1=-40.761:4:0;
YY=spline(x,y,xx);
YY1=spline(x1,y1,xx1);
plot(x,y,'o',xx,YY,'-');
hold on;
plot(x1,y1,'o',xx1,YY1,'-');
hold on;
xy_ik=[xx,xx1;YY,YY1];
for i = 1:22
X = xy_ik(1,i);
Y = xy_ik(2,i);
T_4 = [1 0 0 X;
0 1 0 Y;
0 0 1 0;
0 0 0 1];
qik = quadruped_robot.ikine(T_4,'mask',[1 1 0 0 0 0]);%若没有XYZ三个方向的rotation自由度，应构造一个mask矩阵[1 1 1 0 0 0]
quadruped_robot.plot(qik);%展现逆解的动态过程
qik = qik*180/pi;%转化成度数表示
theta_0 = [theta_0;qik];%将每组数据存起来
pause(0.2);
% alph1 = 180-70-qik(2);
% thata_1 = acos((15^2+35.96^2+25^2-2*35.96*25*cos(pi-alph1*pi/180)-40^2)/(2*15*(sqrt(35.96^2+25^2-2*35.96*25*cos(pi-alph1*pi/180)))))+...acos((l1^2+l1^2+l2^2-2*l1*l2*cos(pi-alph1*pi/180)-l1^2)/2*l1*(sqrt(l1^2+l2^2-2*l1*l2*cos(pi-alph1*pi/180))));
end
测试视频：
arduino 3D打印四足机器人