用MATLAB三步完成机器人搭建

lilybunny

如果说机器人是一粒种子，那么《机器人大擂台》这档节目就是那个播种人。节目中，不同选手操纵着机器人，在擂台上各显神威 —— 冲撞、翻滚、撬杠、喷火等各种技能，总能令人眼花缭乱，同时点燃心中关于机器人的各种幻想。

如今的机器人应用广泛，机器人技术也开始源源不断地向人类活动的各个领域渗透，机器人分拣系统和随处可见的无人机。

例如机器人分拣系统，显著地提升了物流公司货物配送的效率。

另一个例子是无人机。如果说分拣机器人还属于各大物流公司的核心机密，如下凡天仙一般平日里难得一睹芳容，那无人机早已化身邻家女孩，平易近人地装点着我们的生活。

机器人的发展不会止步于此。2020年以来，新基建概念大热，其三大方向更是与机器人产业密切相关。教育机器人、送餐机器人、情感机器人及自动驾驶技术，都是机器人及自动化技术的下一个重要落地目标。

银河补习班里有句经典台词：“我的**，上的是全世界最好的补习班，银河补习班。”今天，MATLAB就为你带来宇宙最强机器人补习班，手把手教你如何建造属于自己的机器人！



许多人认为 MATLAB 是数学或教育软件。事实上，MATLAB 最初被广泛采用是在控制工程领域，随后开发了多种技术领先的算法包和工具箱，并被全球各大高校广泛采纳为数学教育软件。即将进入机器人补习班的你，准备好了吗？

lilybunny

第一步：安装机器人工具箱


工欲善其事，必先利其器，我们首先需要安装 MATLAB 的机器人工具箱（Robotics System Toolbox）。我们既可以上MathWorks 官网下载，又可以通过打开 MATLAB 主界面，单击“添加项”（Add-Ons） 选项进行操作。



然后在搜索栏中输入 “Robotics System Toolbox”，进入第一个搜索结果即可安装。




操作完成后，打开 MATLAB 并输入
>> ver

如果能找到如下信息
Robotics System Toolbox       Version 2.x         (Rxxxxx)

那么恭喜，机器人工具箱已安装成功！

lilybunny

第二步：你想要什么样的机器人？建模是关键

我们要设计的第一个机器人，是一条可以自由转动的机械手臂。在 MATLAB 中，机械手臂由不同的连杆（Link）通过关节（Joint）拼接而成。依活动状态分，关节又可分为旋转型和平移型，其意义顾名思义 。



为简单起见，我们先创建一个三连杆的机械臂：


%设置 DH 参数
dhparams = [0     pi/2  0     0;
            0.4318  0       0       0;
            ];

robot = rigidBodyTree; % 初始化机械臂
body1 = rigidBody( body1 ); % 定义第一个连杆
jnt1 = rigidBodyJoint( jnt1 , revolute ); % 定义第一个关节
setFixedTransform(jnt1,dhparams(1,:), dh );
body1.Joint = jnt1; % 给底座加上关节
addBody(robot,body1, base ) % 定义机械臂为机器人的底座

% 添加第二、三只连杆
body2 = rigidBody( body2 );
jnt2 = rigidBodyJoint( jnt2 , revolute );
body3 = rigidBody( body3 );
jnt3 = rigidBodyJoint( jnt3 , revolute );
setFixedTransform(jnt2,dhparams(2,:), dh );
setFixedTransform(jnt3,dhparams(3,:), dh );
body2.Joint = jnt2;
body3.Joint = jnt3;


上面的 dhparams 是一个 n*4 矩阵（n 是机械臂的个数），被称为 DH 参数（Denavit–Hartenberg parameters）。dhparams 的每一行由 [a alpha d theta] 四个变量组成，它们分别表示：

a: 关节（垂直连杆方向的）偏移量

alpha: 关节扭曲角度（垂直于两个连杆构成的平面的角度）

d: 关节（朝着连杆方向的）伸长量

theta: 关节起始旋转角度（平行于两个连杆构成的平面的角度）

如果不小心忘了我们搭建的机器人长啥样，那么下面的命令

showdetails(robot)

可以帮助我们回忆起它的基本特征：


而输入下面的命令，我们即可一睹该机械手臂的庐山真面目：

addBody(robot,body2, body1 )
addBody(robot,body3, body2 )
show(robot); % 画图



因为我们只搭建了最基本的连杆和关节，所以这个机械手臂难免有些瘦骨嶙峋。不过如果我们把上面的图片放大，就可以看到每个连杆、关节的名字、序号和类型具体信息：



此外，我们还可以给连杆和关节赋予诸如质量、转动惯量、惯性矩阵之类的参数。实践出真知，理解以上概念最好的方式，就是逐个调整代码中的参数，观测结果变化。

lilybunny

第三步：看！它左手右手一个慢动作！

我们已经知道如何用 MATLAB 搭建一个简单的机器人了。然而这样搭建的机器人，和小朋友用积木搭建的玩具并无二致 —— 既然是用软件做成的机器人，我们自然希望它能动起来。我们以加拿大机器人公司 Kinova 的第三代机械臂为例，来看看 MATLAB 如何让这支麒麟臂动起来。



在 MATLAB 界面中输入
>> robot = loadrobot("kinovaGen3");
>> showdetails(robot)

我们可以看到输出如下



不难看出，第三代 Kinova 是由 8 根连杆和 8 个关节拼接而成。如果我们要想知道某个具体连杆的信息，例如名叫 “Shoulder_Link” 的连杆， 依次输入

>> robot.getBody( Shoulder_Link ).Joint
>> robot.getBody( Shoulder_Link ).Joint.JointToParentTransform
>> robot.getBody( Shoulder_Link ).Joint.ChildToJointTransform

即可分别查询属于该连杆的关节，以及关节到母连杆和子连杆的转移矩阵。如此一来，我们就可以从这种方法重建出第三代 Kinova 机械臂了！

现在我们有两种方法可以让让机械臂动起来。第一种方法是通过变换关节旋转角度的方式，这种方法叫正运动学（Forward kinematics）；第二种方法是通过给定每个连杆末端位置，这种方法叫逆运动学（Inverse kinematics）。正运动学可以很方便地对机械臂进行直接操控，而逆运动学则更有利于直接指定机械臂完成任务，例如抓取处于某位置的快递等等。这两种方法各有千秋。

有几何背景的读者知道，n 维欧式空间中的刚体形变可分为平移（Translation）、旋转（Rotation）和镜像（Reflection）三种，这三种变化可以用群作用，或者矩阵来表示。由这三种作用组成的群叫做欧几里得群（Euclidean Group）记作 E(n)，因为 E(n) 中的元素都可以用矩阵表示，所以 E(n) n+1 阶矩阵群的子群（具体解释见下图）。群 E(n) 中只由平移和旋转（不含镜像）构成的子群叫做特殊欧几里得群，记作 SE(n)。



下面我们 MATLAB 中的逆运动学函数 inverseKinematics 来让机械臂动起来。我们只需要定义出机械臂所要经过的几个路径点的位置、速度以及加速度，MATLAB 即可通过对特殊欧几里得群求逆来计算出整个机械臂所经过的路径，以及每个时刻的速度以及加速度等信息。


% 关节起始旋转角及初始位置
positions = [2*pi, 0.2619, pi, 4.0142, 2*pi, 0.9598, pi/2];
config = homeConfiguration(robot);
for k = 1:length(positions)
  config(k).JointPosition = positions(k);
end

% 定义机械臂需要通过的路径点以及对应速度、加速度信息
waypoints = [0.5639 0.0013 0.4336]  + [-0.1 0.2 0.4 ; -0.2 0 0.1 ; -0.1 -0.2 0.4 ;] ;
waypointTimes = 0:4:8;
ts = 0.2;
trajTimes = 0:ts:waypointTimes(end);
waypointVels = 0.1 *[ 0  1  0; -1  0  0; 0 -1  0;] ;
waypointAccels = zeros(size(waypointVels));
waypointAccelTimes = diff(waypointTimes)/4;
[q,qd,qdd] = trapveltraj(waypoints,numel(trajTimes), ...
             AccelTime ,repmat(waypointAccelTimes,[3 1]), ...
             EndTime ,repmat(diff(waypointTimes),[3 1]));

% 画出初始状态
%% 画图--机械臂
show(gen3,config, Frames , off , PreservePlot ,false);
hold on
%% 画图--路径初始化
hTraj = plot3(waypoints(1,1),waypoints(2,1),waypoints(3,1), b.- );
set(hTraj,  xdata , q(1,:),  ydata , q(2,:),  zdata , q(3,:));
%% 画图--路径点
plot3(waypoints(1,:),waypoints(2,:),waypoints(3,:), ro , LineWidth ,2);
set(gca,  Position , [-.2, -.2, 1.5, 1.5]);
xlim([-1 1]), ylim([-1 1]), zlim([0 1.2]);



% 初始化机械臂的逆运动学方程
ik = inverseKinematics( RigidBodyTree ,robot);
ikWeights = [1 1 1 1 1 1];
ikInitGuess = robot.homeConfiguration; % 随机设置一个初始状态

% 让机器人动起来！
for idx = 1:numel(trajTimes)
    % 解逆运动学方程
    tgtPose = trvec2tform(q(:,idx) );
    [config,info] = ik( EndEffector_Link , tgtPose,ikWeights,ikInitGuess);
    ikInitGuess = config; % 以上一时刻的状态作为下一时刻的初始值

    % 画出机器人的动态
    show(robot,config,  Frames , off , PreservePlot ,false);
    title([  Trajectory at t =   num2str(trajTimes(idx))])
    drawnow   
end


最后得到的轨迹图如下：



有兴趣的读者可以修改上面代码中的参数（例如路径点的位置、机械臂的速度加速度等），来看看结果会出现怎样的变化。

lilybunny

更高级的智能

在实际使用中，除了要让机器人会动外，还需要赋予它们自主性。在本节中，我们希望机器人拥有路径规划以及障碍躲避的能力，让我们来一起见证什么是更高级的智能。

我们以即时定位与地图构建算法（Simultaneous localization and mapping，SLAM）作为例子。该算法可以通过著名的卡曼滤波（Kalman Filter）来得以实现。在如雷达、声呐、天气预报、火箭发射、计算机视觉、控制理论等诸多工程应用中，都离不开卡曼滤波的身影。

从抽象角度看来，卡曼滤波无非就是传感器不断根据外部环境，调控机器状态的一种装置。记机器状态 x （位置、速度等），传感器测量的外部环境变量为 z（温度、气压等），可调控的变量为 u （电磁波发射频率、前进方向等），v 和 w 分别表示机器和传感器误差，那么卡曼滤波可以表达为一个数学模型：



k 表示离散的时刻。如果 f 和 h 都是线性矩阵，那直接让上面两式对 x, z 分别求偏导数，然后再更新 x 和 z 的值即可（F_a, H_a 分别表示 f 和 h 对变量 a 的偏导数向量）：



如果 f 和 h 并非线性，由于误差项的累计，直接简单粗暴地线性化势必会带来更大的误差。为此，人们采用预测-更新（Predict-Update）两阶段算法来解决非线性的卡曼滤波问题 —— 在预测阶段，我们不仅要预测机器的状态 x，还要预测卡曼滤波的矩阵 P。这种算法叫扩展卡曼滤波（Extended Kalman Filter，EKF），是 SLAM 的灵魂所在。



P<k> 是卡曼滤波在时刻 k 的协方差矩阵，该矩阵在更新变量 x 时会起到作用

推导过程令人头秃？那我们来看一看代码及实现过程吧！


% 初始化机器人及起始点、终点
% 我们的目的是希望机器人能顺利躲过障碍物并到达终点
map = LandmarkMap(20, 10); % 10*10 地图，20 个障碍物
V = diag([0.005, 0.5*pi/180].^2); % 机器（每个步长）的误差矩阵
robot = Bicycle( covar , V); % 初始化机器人
robot.add_driver( RandomPath(10)); % 定义终点
% 定义传感器
% angle：传感器扫射角度范围
% range：传感器扫射距离
W = diag([0.1, 1*pi/180].^2); % 传感器（每个步长）的误差矩阵
sensor = RangeBearingSensor(robot, map,  covar , W, ...
    animate ,  angle , [-pi/2 pi/2],  range , 5);

% 初始化 EKF， 并运行 150 个步长
P0 = diag([0.005, 0.005, 0.001].^2); % 卡曼滤波初始时刻的协方差矩阵
ekf = EKF(robot, V, P0, sensor, W, []);
ekf.run(150);





上图中蓝色三角形表示机器人，红色菱形表示目标，粉红色扇形表示传感器扫查范围

lilybunny

造机器人？就是如此简单！

读到这里，相信大家对 MATLAB 的机器人工具包已经有了充足的认识。身边无人陪伴倍感孤独？—— MATLAB 帮你打造属于你自己的机器人！

机器人行业快速发展的同时，要面临各种各样的机遇和挑战。MATLAB 的机器人工具包也一样，仍需不断的革新和研发，在以下方面得以改善：
工具包没有覆盖机器视觉方面的内容，因此机器人的传感器只能处理非图像环境信息。要想给机器人加一双眼睛，需要下载计算机视觉工具箱（Computer Vision Toolbox）或图像处理工具箱（Image Processing Toolbox）；
工具包无法利用大数据，这在高度信息化的今天，对机器人的成长是不利的。要想让机器人拥有大数据处理技能，需要统计及机器学习工具箱（Statistics and Machine Learning Toolbox）或深度学习工具箱（Deep Learning Toolbox）。

小时候在看《终结者》、《黑客帝国》等科幻电影时，总认为像“机器人三定律”这样的元素离真实世界还很遥远。

然而随着计算机视觉、机器学习、深度学习等人工智能相关领域的迅速发展，这些科幻电影里的元素会离现实越来越近。