无人搬运自动导引车(AGV)控制系统的实现过程

本文介绍自动导引车(AGV)控制系统的实现过程，分为硬件搭建和软件设计两部分。

1. 车载控制器

1.1 车载控制器的类型

车载控制器是控制系统乃至整个 AGV 的核心，那么应该选择哪种控制器呢?根据笔者的经验，从硬件角度看，AGV采用的车载控制器基本分为以下三种：

PLC：代表产品例如西门子公司的 S7-1200和1500系列，使用 AGV 厂家有佳顺、嘉腾。PLC 以稳定性好、可靠性高著称，但是它的长项是逻辑控制，一般只能做简单的运动控制。西门子 PLC 的价格大概在2000元~12000元左右。

工控机：代表产品例如倍福公司的 CX5130，使用厂家有昆船、罗伯特木牛流马。工控机其实就是一台电脑，只不过经过精心的设计使其具有很强的抗干扰能力，可以用于振动、电磁辐射等恶劣的工业环境。工控机结构紧凑、体积小巧，其可以运行通用的操作系统，比如 Windows、Linux。以倍福为例，安装它的 TwinCAT 控制软件后，会在 Windows 操作系统中插入实时内核，将电脑变成适合工业场合的实时控制系统。倍福工控机的售价大概在5000到30000元左右。

单片机：代表产品例如 STM32，使用厂家有海通、机科。相比前两者，单片机的价格低廉，加上外围器件成本最多也不过千元左右。使用单片机的一个缺点是，如果要做成成熟的产品，你需要花费很多精力设计外部电路以及各种底层算法。软件开发环境可能会差一些。

上面的分类并不意味着三者是泾渭分明的，例如 PLC 的核心芯片可能就是一个单片机;而工控机的编程语言很可能就采用了 PLC 领域使用的梯形图、ST等语言;单片机上面也可以装载操作系统，从而形成一个完整的控制器。所以，虽然它们各有自己适用的领域，但是相互之间也会取长补短，演变成你中有我我中有你的局面。

当然，现在市场上有专门的AGV控制器，例如美国NDC的CVC600，瑞士BlueBotics的ANT、浙江科聪智能的 MRC5000，上海仙知机器人的SRC。与通用的工业运动控制器或PLC相比，这类控制器集成了成熟的导航和运动控制算法，为用户省去很多工作，而且其稳定性和防护等级更高，但其价格也较高(2万~6万元)。

本文选择西门子的 S7-1215C 型 PLC 作为 AGV 的车载控制器(以后简称为S1200)，如下图所示。由于西门子的产品型号非常丰富，各个型号在接口种类、性能等方面有细微的差别，在购买时为了准确描述具体是哪个型号，最好使用订货号。西门子每种产品都有一个唯一的订货号，在采购时只需要提供这个编号即可。在西门子的编程软件中也可根据订货号查找对应的型号。

什么是上位机和下位机?

有些设备能够访问其它的设备，比如读取内存中的数据、向内存写数据，这样的设备具有较高的权限和地位，因此被称为上位机。有些设备只能被其它设备访问，而没有访问其它设备的权力，这样的设备就称为下位机。在本文中，PLC就是下位机，而编程计算机就是它的上位机。

想对PLC进行设置或编程，需要使用网线将 S1200 与上位机连接，只需要将上位机的 IP 地址设置为与 PLC 同一网段即可。S1200 一般出厂默认的 IP 地址是 192.168.0.1，所以可以将你的电脑的 IP 设置为 192.168.0.2。对于 AGV 这种移动设备，通常会采用无线通讯，例如接收调度系统的控制指令。无线通信方式有 WIFI、ZigBee等等。由于 WIFI 在实际中使用较多，我介绍一下它的配置方法。使用 WIFI 连接西门子 PLC 非常方便，只需要用网线将 PLC 的以太网口连接到无线路由器的 LAN 口，然后用笔记本搜索到对应的 WIFI 后连接即可访问 PLC。所用的无线路由器可以是任意品牌，甚至家用路由器都可以。

2. 驱动器

如果说车载控制器是 AGV 的大脑，那么 AGV 还要有心脏和肌肉才能运动起来，驱动器给车轮的电机提供电流，相当于心脏。

2.1 低压驱动器品牌

由于 AGV 普遍采用蓄电池供电，其电压一般在 12V~72V 左右。因此 AGV 使用的驱动器属于低压直流型伺服驱动器。国外的低压驱动器品牌有：AMC、RoboteQ、ZAPI(萨牌)、Curtis(柯蒂斯)、Elmo(埃莫)等，中国国内的低压驱动器品牌有：步科、森创、英博尔、凯利。

2.2 PLC与驱动器通信

PLC 要想控制驱动器就要向它发送指令数据。低端驱动器可以用模拟量或者脉冲信号传递数据，高端驱动器则采用规范的通信协议传递数据，这样的通信可以借助 RS-232、RS-485、CAN、EtherCAT 等协议实现。因为 S1200 的 CPU 模块没有这些通讯接口，所以要想连接驱动器，需要购买额外的通信模块(CM：Communication Module)。上面提到的几种协议西门子都有对应的通信模块支持，比如西门子的 232 、485 和 CANopen 通信模块如下图所示(这三种模块外形完全一样)，注意 232 和 CANopen 模块的插头是公头，而485的则是母头。

 

对于移动设备来说，CAN总线是最常用的通讯方式，像汽车、挖掘机、AGV一般都采用CAN总线通讯。CAN 总线的速度快(最高可达1Mb/s，比485要快)、稳定性高(采用差分信号，抗干扰)，更“智能”(有优先级和仲裁机制)，因此应该优先选择 CAN 总线。但是考虑到西门子本身没有 CAN通信模块(如果想使用CAN，要购买第三方公司HMS提供的CANopen模块)，本文使用的驱动器采用 485 协议，所以我们选择485模块。随之而来的一个问题是：S1200能外扩几个通信模块?答案是：3个。不管是哪种通信模块，数量最多只能是3个，种类不限可以混用。如果你的驱动器是总线型(485和CAN都属于总线型)的，那么只要一个485模块或者 CANopen 模块就行了。注意232不是总线型的，所以用两个驱动器就要添加两个通信模块。如果你的 AGV 还有其它走通信协议的传感器(例如磁导航传感器、RFID传感器)，那么3个通信模块很有可能就显得捉襟见肘。

西门子485模块的接头采用了标准9 针D型插孔式连接器。为了将驱动器和PLC连接起来，我们要购买DB9接头，并用信号线将两个头连接起来。这里我们只利用两个信号线即可。值得注意的是，西门子的485接口定义和常规的485不一样，常规485的信号收发是1、2头，而西门子的485则是3、8头(参考手册的第1769页)。所以在连线时，要按照 3-2、8-1的对应顺序焊接，如下图所示。

3. 移动机构

AGV要想实现搬运、移载就要有移动机构，通常采用的移动机构有舵轮、差速轮和麦克纳姆轮。它们各自有什么特点呢?什么场合又该使用哪种轮子呢?

麦克纳姆轮的优点是运动精度高，但是结构复杂，磨损严重，对地面挑剔，虽然麦克纳姆轮问世超过30年了，但是实际应用并不多，这也说明它不是一个实用的产品。下面我们主要讲解舵轮。

舵轮是个高度集成的移动机构，由车轮、回转支撑、齿轮箱、驱动电机、转向电机、编码器、限位开关、制动器等部件组成，它既能提供牵引力，也可以主动转向，因此一个舵轮有两个自由度。AGV 使用一个或以上的舵轮就可以实现全向移动：原地旋转、斜行、横移等等。舵轮一般用于室内环境，所以轮面的材料多为聚氨酯。根据电机的安装位置可以将舵轮分为两种：卧式和立式，分别如下两图所示。卧式舵轮的驱动电机横向安装，因此整体高度很低，有些AGV对高度有要求(例如潜入式的AGV要很矮才能钻到货物底下)，这时就适合使用卧式舵轮;立式舵轮的驱动电机竖着安装，其整体高度偏高，它的优点是方便将电机隔离起来，在有些场合(例如危险品工厂)，要求舵轮电机与外部环境隔离，这时就适合使用立式舵轮。国外主要的舵轮生产厂家有：意大利的 CFR 和德国的 schabmuller(夏伯穆勒)，后来从CFR中又独立出来了一个厂家——马路达。这些舵轮生产厂家一般只卖舵轮，不提供驱动器，驱动器则由代理商提供。一台AGV上最贵的部件就是舵轮，以一台成本10万元左右的AGV为例，如果配备两个舵轮，就需要4~5万元，占整车成本将近一半。不过好消息是，国内的舵轮厂家开始参与竞争，这将降低AGV的成本。

 

4. 磁导航传感器和磁条

4.1 磁条

磁条导航是目前 AGV 使用最多的导航方式之一。只需要预先在地面铺设好磁条，AGV 就会沿着磁条形成的路径运动。磁条一般宽3~5cm，它像胶带一样，一面光滑、一面带有粘性，将粘性的一面粘贴在地面即可完成铺设。磁条具有极性，在购买时注意是N极还是S极，磁条的极性对应磁导航传感器的极性，但有些传感器两种都兼容。磁条质地较软、不抗压，所以如果被轮子反复碾压可能会压扁或挤脱落。多数 AGV 为了避免压坏磁条会将驱动轮布置在车体的两侧(差速形式)或者斜对称的位置上。

4.2 磁导航

磁导航传感器也叫磁循迹传感器，它用来帮助AGV巡线行走。磁导航传感器一般安装在AGV车体或者驱动轮组上，在AGV运动时始终保持在磁条上方一定高度内。磁导航传感器相对于车体的安装位置一般在对称中轴线上，通常对称安装，例如前后各一个或者左右各一个。磁导航传感器的原理很简单，传感器上有若干个探头，每个探头都能检测磁场，位于磁条上方(一定高度内)的探头有信号输出，超出磁条的探头则没有信号。探头通常俗称为“点”，点越多表示传感器的分辨率越高，当然价格也越贵。传感器通常的点数有：6点、8点、16点几种。磁导航传感器并不是什么神秘的东西，笔者曾经拆解过一款磁导航传感器，这里所说的探头无非有两种，一种是霍尔元件、还有一种是地磁传感器。霍尔元件的特点大家大学的时候应该都学过，它能感应磁场，价格低但是精度通常不高;地磁传感器基本都采用美国 PNI 公司的产品。它要搭配专用的驱动芯片 PNI-12927，这个芯片是3.3V供电，所以传感器内部要设计3.3V稳压电路，而且这个驱动芯片采用 SPI 总线，要对其信号进行处理和电压变换，这通常由单片机来完成。磁导航传感器的输出信号可以是数字量也可以是总线型数据，对于点数多的传感器来说一般用总线型，因为如果使用数字量需要很多根线。假设一个 AGV 使用4个传感器，每个传感器的分辨率是16点，那就要有4×16=64 4\times16=644×16=64个输出，相应的 PLC 应该有 64 个数字量输入口，其接线将非常繁琐，也容易出故障(例如某根线连接不牢固)。

磁导航方式实现简单，价格便宜，但是缺点也不少，其中一个严重的问题是干扰。笔者在调试 AGV时发现，即使传感器并不在磁条上方(有时甚至周围根本没有磁条)，传感器依然有输出。经过一番思考，笔者发现了问题所在。AGV 前方有一条铁轨，而铁是容易被磁化的。不仅是铁制品，通电的导线也可能会使传感器误报。所以，使用磁条导航要排除这些干扰。

磁导航传感器的输出可以是IO，也可以是数值(代表传感器偏离磁条中心的距离值)。　磁导航传感器如何与 PLC 连接?如上所述，如果传感器采用总线(例如485)传递数据，直接将总线连接到相应的(485)通信模块即可。如果传感器采用数字量接口，那么就要连接到 PLC 的数字量输入口。这里有个很重要的问题：你要清楚传感器输出的信号是 PNP 型还是 NPN 型，它对应了三极管的类型，它们的区别在于有信号时 PNP 输出高电平(例如24V)，而 NPN 输出低电平(例如0V)。在 S1200 上每组输入输出口旁边都有一个 COM 口，它的作用就是选择输入是 PNP 型还是 NPN 型。如果是 PNP 型，COM 口就要接 24V ，如果是 NPN 型就要接 0V。你可以找个磁条用万用表测试一下，如果放到磁条上输出显示是24V那说明是 PNP 型。以后你会知道，很多传感器都分 PNP 型和 NPN 型，它们的接法是一样的。

5. 供电系统

5.1 电池

AGV启动时需要的电流大，因此需要使用动力电池，使用最多的动力电池有铅酸电池和锂电池。铅酸电池寿命短、能量密度低，而锂电池密度高、但是成本也高。还有一种电池叫做超级电容，它的寿命很长，但是能量密度非常低，所以用的不多。

知名的电池厂家有霍克(HAWKER)、荷贝克(HOPPECKE)等。下图是霍克 AGV 专用电池标签。铅酸电池的额定放电电流一般是0.5C，例如80Ah的电池，额定放电电流是0.5×80=40A 0.5\times80=40A0.5×80=40A。铅酸电池的最大放电电流一般是3C，即3×80=240A 3\times80=240A3×80=240A。电池的电压会随着电量消耗逐渐降低，开始降低得很慢，当电量很少时会急剧下降。根据这个关系，通过检测电压就能知道电池还剩余多少电，是否应该充电。

我们如何估计一个 AGV 该选用多大容量的电池呢?AGV 上最耗电的器件就是电机了。一般来说，电机的额定功率 P会写在电机铭牌上。根据初中物理知识，功率等于电压和电流的乘积，即：P=UI

驱动电机的额定电压一般容易得到，所以通过上式可以计算出电流。当然，实际工作的功率一般会比额定功率小，所以这里要乘以一个系数，具体要看 AGV 的负重、工作路面等。求出电流乘以工作时间就是电池的容量了。注意，电池的容量和能量是两个不同的概念，前者是电流乘以时间(单位是“安时”(Ah))、而后者是电流乘以电压乘以时间(单位是“千瓦时”(KWh))。

5.2 电压变换

如果传感器、控制器等器件和电池的输出电压不同，就需要使用电压变换模块，它还可以起到稳压的作用。一般传感器常用的电压是12V、控制器常用的电压是24V，因此需要有相应的模块。可以选择广州金升阳的电压变换模块，可选配散热片和导轨，便于散热和固定，如下图所示的48V转24V的模块。选择模块时主要关心的功率，即它能提供的最大电流。

6. 开关

6.1 电源开关

选择电源总开关时要注意它是“自锁的”还是“自动复位的”。自锁的意思就是按下后即使手指松开，开关依然保持闭合使供电畅通;而自动复位的开关按下按钮后如果松开手指，电路就会被切断，如下图所示。一般机箱上的电源总开关应该选择自锁型的，否则你要一直按着按钮才能使电路畅通，因此不要购买下图右所示的自复位型开关(代号：XB2-BA31C，价格在22元左右)。假如你买错了也不要紧，你可以再买一个自锁模块ZB2-BZ21 C，将这个模块添加到XB2-BA31C开关上就能将它变成一个自锁开关。

 

6.2 急停开关

在所有AGV 车体上都能找到的一个东西就是急停开关。根据国家标准GBT 20721-2006 《自动导引车 通用技术条件》中第4.2.6项的规定：“AGV 应配备紧急停车按钮，该按钮的安装位置在紧急情况下应便于操作。应采用红色按钮，急停按钮按下应关闭 AGV 驱动电源”。在AGV失去控制的情况下，按下急停开关能让它立即停止运动，从而防止伤害到人。因此急停开关构成了最后的安全屏障。为了保证使用可靠，可以购买施耐德的XB2-BS542C。急停开关的按钮一般设计得很大，方便按下，因此又被俗称为蘑菇头，它一般都是自锁的，按下后需要旋转才能松开。购买时唯一要注意的是它的安装直径，一般选择 22mm 即可。

7. 电气元件

AGV上既有强电也有弱电，例如舵轮的驱动器需要的电流通常很大，而控制器和一些传感器只需要很小的电流。我们要合理设计电路，防止它们相互干扰，更主要的是包含弱电器件。根据国家标准GBT 30029-2013 《自动导引车(AGV)设计通则》中第6.7.7.4项的规定，控制器应该具有过压保护、欠压保护、过流保护功能。

我们要在电路中加入以下器件：

接触器：用小电流控制大电流。上面的电源开关只允许通过较小的电流，但有时设备里有些组件需要大电流才能启动，例如舵轮驱动器。我们不能用一个开关既控制小电流也控制大电流，这时就需要借助接触器。接触器内部有个电磁铁，我们用小电流控制电磁铁的吸合，电磁铁带动另一组开关闭合使大电流通过。在AGV上使用的是直流接触器，注意接触器的线圈会发出一定热量，安装应当注意散热和对附近器件的影响。

断路器：起到过流、欠压保护的作用。断路器也相当于一个开关，在短路等电流过大时断开，保护设备;

继电器：也相当于一个开关，它是用小电压控制触点开闭的，但是注意触点不能流过太大电流。

AGV 采用电池作为能源，而电池的电压通常不是很稳定，在使用时电压会波动。虽然西门子的 PLC 对输入电压具有一定的适应能力，但是我们最好用一个24V到 24V 的稳压模块将电池的电压稳定后再供给 PLC。对于使用24V供电的其它器件，例如磁导航传感器，也是这样。

8. 激光避障传感器

AGV的安全防护方式有接触式和非接触式之分。触边属于接触式的，而激光传感器和超声波传感器属于非接触式。当然非接触式的传感器更加安全，因为不需要直接接触即可监测障碍物。

常用的国外激光传感器品牌有SICK和HOKUYO。以SICK的Tim310为例，它的监测距离可达3米，可以设置不同的监测区域形状。Tim310的输出是I/O信号，用以判断是不是监测到了障碍物。 

9. 触摸屏

现在的 AGV 越来越多地开始配备触摸屏了。使用触摸屏可以方便地设置AGV的参数，例如运动速度、充电电压等等，而不需要每次都将程序下载到 PLC。既然我们选择了西门子的PLC，当然触摸屏选择西门子是最配对的，但是考虑到西门子的触摸屏有点贵，我们选择国产的品牌。国产触摸屏的质量也很可靠，而且价格合理，代表厂家有：威纶通、昆仑通态。由于西门子的知名度，大多数触摸屏都会支持。但是如果你用其它品牌的PLC，那最好向触摸屏厂家询问是否支持你的PLC，否则是无法通讯的。

我选择昆仑通态的触摸屏，它采用24V直流供电，并配有网口，用普通网线将触摸屏和 S1200 连接，并将触摸屏的 IP 地址设置为与 PLC 所在同一网段(例如192.168.0.3)即可实现和 PLC 的互联。设置 IP 的方法是上电后在进入正式界面之前，在屏幕的任意位置点击一下即可进入设置界面。

我们如何使用触摸屏呢?我们需要对触摸屏进行编程，或者说“组态”。编程需要使用昆仑通态自己的组态软件：MCGS，可以在它的官方网站免费下载。

在你的电脑上安装该软件，注意在软件中需要输入 PLC 的地址才能读写数据。触摸屏上只有一个网口，所以你可以用网线将触摸屏连接到你的电脑。我们注意到 S1200 有两个网口，所以一种更方便的连接方式是，将触摸屏连接到 S1200 上，然后将 S1200 连接到你的电脑，这样同样能对触摸屏进行编程。　　触摸屏最基本的功能就是读写 PLC 的内存(因此它也是PLC的上位机)。要实现这个功能非常简单，只需要在 MCGS 中对 PLC 的变量进行关联即可。

10. PLC编程

10.1 编程工具 STEP 7

购买了S1200 并不意味着你能免费使用它的开发软件。但是没关系，网上有很多破解软件。有意思的是，其中有些资源就是西门子公司的人故意释放出来的，他们可能想占有更多的市场。要对西门子的 PLC 编程必须使用专用的软件：STEP 7 ，下图是 STEP 7 的安装过程。

实际上，STEP 7 是过时的名称，现在西门子想将它和另外几个自动化软件都统一成一个平台，适用于西门子旗下所有的设备，这个平台被称为：TIA Portal(博途)，本文使用的就是 TIA V13。西门子公司为了保持延续性，软件的名称有时是 STEP 7，有时又用 TIA，本文我为了与历史统一还叫 STEP 7 好了。西门子的软件跟硬件一样种类繁多，光 STEP 7 就有几个版本，例如专业版(professional SP1，如下图左所示)，而每个版本又有不同的更新包。注意 SP1 可能缺少某些功能(下面我会提到)，因此我使用了更新包。具体来说，我使用的是 STEP 7 V13.0 SP1 Upd8，如下图右所示。

在软件安装时我们注意到有这样的说明：STEP 7是用于编程的工程组态工具。要使用 PLC，编程只是一方面，还有一个重要的工作就是“组态”。“组态”说白了就是“配置”，配置 PLC 的输入输出接口、参数、功能等等。首先，在“设备组态”页面选择我们的 PLC 型号就可以了。

10.2 西门子PLC的硬件资源要想用好PLC，就要熟悉它的一些特点，下面我们就了解一下 S1215C 的特点。1215C的内存有多大呢?我们用电脑连接上PLC，然后在博途软件左侧栏双击“在线和诊断”，然后在打开窗口中点击“存储器”，如下图所示。

可以看到这里有三个存储器，分别是：装载存储器、工作存储器和保持存储器。并且可以看到三个存储器的大小，分别是：

1. 装载存储器：　4194304 41943044194304 Byte ÷1024÷1024=4 \div 1024 \div 1024= 4÷1024÷1024=4MB

2. 工作存储器：　128000 128000128000 Byte ÷1024=125 \div 1024= 125÷1024=125KB

3. 保持存储器：　10240 1024010240 Byte ÷1024=10 \div 1024= 10÷1024=10KB

这和系统手册中给出的数值是一样的，如下：

这三个存储器各有什么作用呢?

1. 装载存储器：用于存储用户项目文件(例如：用户程序、组态信息)，相当于硬盘;

2. 工作存储器：CPU在执行用户程序时会将一些项目内容从装载存储区复制到工作存储区，相当于电脑的内存;

3. 保持存储器：掉电后数据仍然保持。如果你希望一些数据能在断电后一直保持就要使用这个存储器。但遗憾的是，小气的西门子公司只给了我们10KB。

PLC 扫描一次程序需要多长时间呢?

PLC会自动记录时间，在同一页面，打开“循环时间”，就能看到PLC完整执行一遍程序需要的时间，如下图所示。这个时间不是固定的，取决于你的程序长短，一般的执行时间在3毫秒以内。

10.3 西门子PLC的软件资源

西门子提供哪些函数运算呢?工业控制语言唯一的国际标准——IEC61131-3 规定了46个基本函数，西门子都支持。除此以外，我们常用的还有定时器等。AGV 属于移动机器人，而机器人的控制本质是数学计算，所以我重点说说其中的数学函数，共有18个：

数字运算：ABS、SQRT、LOG、LN、EXP、SIN、COS、TAN、ASIN、ACOS、ATAN;

算术运算：ADD、SUB、MUL、DIV、MOD、EXPT、MOVE;

当然还有：>、< 比较运算符。

翻开另一款控制器品牌——倍福的手册，你会发现它同样支持这几个初级函数。在组态界面，双击 CPU 模块能看到这款 PLC 的其它功能介绍，例如周期定时器，如下图所示。

如果你使用了触摸屏，有一点值得注意：在“保护”页面中有一个选项需要勾选：“允许从远程伙伴使用PUT/GET通信访问”，如下图所示。如果不勾选这个选项，你看你无法用触摸屏读写 PLC 内存中的数据。让笔者奇怪的是，像这样的细节几乎在所有的手册、教程中都找不到，但却又很重要。

10.4 西门子PLC的编程方式

西门子PLC 的编程符合国际工业编程语言标准 IEC-611313，因此跟倍福等厂家的差不多。

10.4.1 定义常量

有些量我们要经常使用(例如圆周率 π \piπ)，如果每次都用数值来做运算一方面输入很容易出错，另一方面如果想对其进行更改那要改很多处，非常繁琐。一个方便的做法是将其定义为一个全局的“常数”。使用 PLC 和 C 语言等高级语言相同的一点是，你需要为变量分配内存地址。分配好地址后如何对其赋值呢?因为常量只需要赋一次值即可，所以我们选择在 Startup 块中对其赋值。Startup 块在每次 PLC 上电后只执行一次。西门子中有些块的序号是系统预留的，比如 Startup 就是 OB100。我们注意到，在博图软件中，全局变量用双引号表示(例如"PI")，而局部变量前面带个井号(下面会碰到)。

 

10.4.2 定义函数

西门子自带的函数只能完成简单的功能，为了实现更高级的控制我们还要自己写一些函数。笔者在开发机器人控制器的过程中发现，网上公开的PLC函数库很少，目前笔者只知道OSCAT库。大家都在讨论基本的逻辑控制，却鲜有人讨论更高级的数学计算，可能 PLC 并不适合高级运动控制算法的实现。

下面我用一个很简单的进行角度弧度转换的函数为例来说明如何在 STEP 7 中自定义函数。

(1) 首先，我们新建一个函数FC(这个函数比较简单，不需要背景数据，所以不用 FB)，实现的语言选择 SCL 语言。函数名为“DegreeToRad”。

(2) 然后，我们要明确这个函数的输入，在上面的变量定义栏中的 Input 项目中输入 angleInDegree，类型选择为 Real，如下图所示。在Return 项目中将 DegreeToRad 变量的类型改为 Real，这里注意不要修改 DegreeToRad 变量的名字。

(3) 在下面的输入框输入变换公式。　　这里要用 := 对变量进行赋值。你会注意到，变量 PI 是用双引号括起来的，而其它的变量(例如 angleInDegree)前面则带一个井号。这是用来区分局部变量和全局变量的，局部变量用#开头表示，而全局变量用引号表示。

10.4.3 定时器

定时器是我们会经常使用的一个功能，想输出随时间变化的量、想延时触发、想输出周期信号都要借助定时器。西门子的每个定时器都对应一个背景数据块。笔者早期经常犯的一个错误是，为了节省时间直接复制粘贴定时器块。其错误是不管你复制多少次，其实最终还是同一个定时器，而且后面的会覆盖前面的值。这个错误导致很多匪夷所思的错误，让笔者头疼了相当长的时间。各位一定要注意，每使用一个定时器都要重新拖进来新建。

10.4.4 磁循迹算法

为了让 AGV 始终沿着磁条行走，我们要编写相应的控制程序，也就是循迹程序。循迹程序的逻辑可以非常简单，我们的输入是磁导航传感器的读数，输出则是舵轮转向的速度(或差速轮的转速差)，磁条如果在 AGV 车体中心线的左侧，我们就往左偏转，反之在右侧就往右偏转，而如果恰好在中间则保持不动。下面的仿真动画展示了这种控制策略的效果，其中只使用了一个传感器和一个舵轮，但已经可以沿着曲线行走了。磁导航传感器有六个点，安装在前方，红色的方格表示感应到了磁条，白色表示没有感应到。

审核编辑：汤梓红