CN109079738B - 一种自适应agv机器人及自适应导航方法 - Google Patents

Abstract

本发明属于机器人技术领域，公开了一种自适应AGV机器人及自适应导航方法,包括车体、设于车体上的导航装置和设于车体上的执行装置；导航装置包括探测雷达、探测摄像头和处理器，用于机器人行进路径的规划，主动避让路径上的障碍；执行机构包括底座、协作机械臂、执行舵机，协作机械臂包括第一驱动箱、四连杆机构、第二驱动箱、套轴机臂、末端传动头和末端执行头，由此形成六自由度的执行结构。在车体承载、导航装置的引导下，机器人可在室内环境下自由行走，驱使执行机构执行任务。本发明自重小，成本低，可以实现比同类协作机器人更灵活的路径规划，协作机械臂能有更长的臂展和更大的负重，并具有良好的可扩展性。

Description

一种自适应AGV机器人及自适应导航方法

技术领域

本发明属于协作机器人技术领域，具体涉及一种自适应AGV机器人及自适应导航方法。

背景技术

AGV(Automated Guided Vehicle，自动导引运输车)是指装备有电磁或光学等自动导引装置，它能够沿规定的导引路径行驶，具有安全保护以及各种移载功能的运输车。AGV广泛应用于仓储搬运以及制造搬运，较为先进的AGV还应用于危险场所和特种行业。

现有的AGV车架基本采用轮式移动、带式移动等结构，便于实现车体在狭窄空间内实现自由行进和转向。但目前的AGV车架行进和转向方式基本为铰轴式转向，在这种转向机构中存在其最小转弯半径，在应用空间上有一定的限制，在多数较为局限的空间内，采用铰轴转向机构的AGV难以一次顺利通过，甚至无法通过，导致AGV的应用面临较大的问题。

而协作机器人的机械臂是一个多输入多输出、高度非线性、强耦合的复杂系统。因其独特的操作灵活性,已在工业装配,安全防爆等领域得到广泛应用；随着机械臂的伸入研发，其结构和精度均得到极大的改善，已不仅仅在目前的领域应用，还拓展至诸多直接辅助人进行操作的技术领域，甚至能部分代替人的工作，在诸多工业领域，包括敏感环境和复杂环境，协作机械臂的运用能够保护人员、保证生产质量、提高效率和减少设备损坏。例如在AGV(Automated Guided Vehicle，自动导引运输车)技术领域，协作机械臂能够与AGV配合使用，参与生产自动化的各项工序，例如码垛、分拣、视觉检查等。

当下的协作机械臂要求速度、刚性、工作半径、负荷、定位精度等指标尽可能达到更高的标准，从而能够参与更多领域的工作。协作机械臂一般包括多个工作轴，通过相互配合实现多个自由度的协调运转，达到精确控制方向的目的，最终实现操作指令动作。

目前市场上能与人共存、不用专门划分安全工作区域的小型机械臂大致分为两类。

第一类是基于步进电机或者舵机的小型机械臂。此类机械臂由于使用步进电机或者舵机，其控制原理是开环控制，即如果电机由于干扰或负载等原因没有达到指定目标，机械臂无法得知此反馈，因此无法进行调整以消除误差。这就完全限制了此类机械臂的运动性能，即只能实现从一个点到另一个点的移动任务，而不能精确地从位置、速度、以及加速度三个指标上跟随一条完整的轨迹。此外在带有额外负重后，由于仅仅采用开环控制，该类机械臂可能会有较大的稳态误差。另外，由于采用开环控制，此类机械臂无法实现碰撞检测这一功能，因此为了能与人安全共存，只能通过降低功率，减小尺寸来确保即便碰到了人，也不会造成太大伤害，这导致此类机械臂多用于轻负载的视觉检查，电子元件分拣，以及儿童智力开发等领域，无法承担广泛的工业与非工业的自动化任务。

第二类是叫做“协作机器人”的机械臂。这类机械臂精度高，运动性能好，且由于能与人共存，被越来越多应用在各类生产流水线上。目前市面上这类协作机械臂全部采用“力矩电机”或者“盘式电机”。此种电机的优点是扭矩大，可以直接驱动其负载，而无需任何传动装置，如齿轮、皮带等。但是这类电机最大的缺点是成本高，且成本与扭力大小不成线性关系。因此基于力矩电机的协作机械臂，出于成本考虑尺寸都被迫限制在一个范围内，因为一旦超出此范围，就需要搭载成本高得多的力矩电机。此外，这类不使用传动系统的设计结构还有一重大缺点，就是可延展性很差。这是因为由于没有传动系统，每一个电机都必须安装在关节本身的位置，这就导致低位的电机需要承受所有高位电机的质量。例如低位轴电机不仅要承担末端负载的质量，还要承担高位轴电机的自重。这使得机械臂的最大负重很难提高，因为要提高最大负重，就要增加每一个电机的功率及自重，但因此带来的是每一个低位电机承受的高位电机的自重增加了，结果就是增加的功率有很大一部分还是用在了承载电机自重上。

而现有的AGV导航技术，主要包括利用磁导轨、激光探测或惯性等技术的导航方案，利用磁导轨技术的方案对场地要求极高，且后期变更和调整极为困难；利用激光探测和惯性技术的导航方案，所依赖的设备成本极高。

因此，在本领域为了存在上述的一些技术问题，需要提出更为合理的技术方案予以解决。

发明内容

本发明提供了一种自适应AGV机器人及自适应导航方法，旨在将协作机械臂固定到车体上，通过车体带动协作机械臂位移，能实现协作机械臂及时到达指定区域执行任务。车体通过自适应导航方法，能够主动在巡检区域内行走，并识别前路中的路况，分析和辨别前路中存在的障碍，实现主动绕行避障。

为了实现上述效果，本发明所采用的技术方案为：

一种自适应AGV机器人,包括车体、设于车体上的导航装置和设于车体上的执行装置。

具体地说，所述的导航装置包括处理器、探测雷达、探测摄像头、第一过滤器和第二过滤器，探测雷达、探测摄像头、第一过滤器和第二过滤器分别与处理器相连。

所述的执行装置包括底座、设于底座上拥有六自由度的机械臂、设于机械臂末端的执行舵机，所述的机械臂包括依次连接的第一驱动箱、四连杆机构、第二驱动箱、套轴机臂、末端传动头和末端执行头。具体地说，所述底座上设有相对其自身转动的第一转轴，所述第一驱动箱与第一转轴的上端连接固定，所述底座上设有蜗轮蜗杆驱动组件，蜗轮蜗杆驱动组件连接第一转轴的下端并同步传动；所述第一驱动箱内设有第二转轴和第三转轴并分别由第二电机和第三电机驱动，所述四连杆机构围成平行四边形且与第二转轴和第三转轴配合传动；所述第二驱动箱固定在四连杆机构的上端，第二驱动箱包括第二箱体，第二箱体的后表面设置有两个直流有刷电机，第二箱体内设有分别与两个直流有刷电机连接的减速传动机构，所述套轴机臂包括分别与减速传动机构连接传动的内层输出轴和外层输出轴，外层输出轴连接末端传动头并带动其同轴转动，内层输出轴伸入末端传动头内部并通过一级齿轮副与末端执行头连接传动，使末端执行头与末端传动头相对转动。

本发明以车体作为机器人的行走系统，通过车体搭载执行机构行进，并利用车体上的导航系统实现自适应导航；执行机构通过涡轮蜗杆驱动组件、第一驱动箱和第二驱动箱作为整个机械臂的动力机构，以四连杆机构、轴套机臂和末端传动头作为传动机构，以末端执行头连接末端执行器实现最终的执行动作。

其中，关于车体部分，所述的车架包括基架，基架包括矩形的外缘条和设置于外缘条内的方格状中网，所述中网包括密集层和疏散层，所述疏散层贴合在密集层的下部，疏散层和密集层的厚度相同且均为外缘条高度的一半。

所述车架的中心位置沿宽边方向设有差分驱动装置，差分驱动装置包括主控板和两个相对设置的方形的电机座，主控板可设置在车架上，也可设置在车架上搭载的其他控制器内，当设置在其他控制器内时，便于集成化控制管理；电机座内设有与主控板电连接的电机，电机转轴连接减速器，减速器连接到驱动轮，两个电机分别驱动一个驱动轮，当两个电机的运动状态同步时，可实现AGV的前进或后退，当两个电机的运动状态相互不同步时，可实现AGV的转向；所述车架两个宽边的中部设置有前导向组件和后导向组件，所述前导向组件和后导向组件分别包括前轮座和后轮座，前轮座上设有前导向轮，后轮座上设有后导向轮。

进一步的，车架上需要安装承载AGV的导航装置和协作机械臂，且各部件在车架上的布局会影响AGV的运作，为了方便自由设置各部件，对其位置进行合理的布设，对上述方案进行优化，所述电机座、前轮座和后轮座上均设置有竖直向上的连接卡头，连接卡头包括卡入疏散层的第一卡头和卡入密集层的第二卡头，第二卡头位于第一卡头的顶端，且第二卡头的顶端设置有螺孔。

再进一步，疏散层和密集层对应的网孔尺寸不同，疏散层的网孔尺寸大于密集层的网孔尺寸，增加连接固定点可提高安装的稳定性，因此对上述技术方案进行优化，所述的第一卡头顶端设有至少两个第二卡头。

再进一步，电机座的尺寸较大，且安装驱动力部件，容易松动，因此需要更多的点受力进行稳定；而前轮座和后轮座的结构尺寸较小，且仅安装从动部件，其稳定性比较高；故对上述的技术方案进行优化，所述的电机座上的连接卡头数量大于二，且均位于电机座的侧表面，前轮座和后轮座上的连接卡头分别位于其上端面。

进一步的，所述的疏散层和所述的密集层均包括横向格条和纵向格条，横向格条与纵向格条相互垂直，横向格条与纵向格条的厚度相同。

再进一步，所述的横向格条平行于外缘条的长边方向，且密集层相邻横向格条之间的间距等于横向格条的厚度。

以上内容从结构上对车体进行了说明，当具体应用时，差分驱动装置的两个电机分别受主控板的控制转动，电机对应的两个驱动轮转动参数也有差异，以此能够实现差分转向；当两个驱动轮的转动参数相同时，可实现前进和后退。

关于协作机械臂部分，所述协作机械臂的底座连接固定在车体的上表面；所述涡轮蜗杆驱动组件包括第一电机，第一电机上连接蜗杆，蜗杆与设于第一转轴上的涡轮配合传动，第一电机运转后通过蜗轮蜗杆带动第一转轴转动，从而实现机械臂在水平方向的转动；第一驱动箱内的第二转轴和第三转轴实现带动四连杆机构的运行，实现第二驱动箱在纵向上的圆周运动和俯仰动作；第二驱动箱上的直流有刷电机通过减速传动机构控制套轴机臂的内外输出轴运转，外层输出轴的转动带动末端传动头和末端执行头产生同轴转动，而内层输出轴的转动可带动末端执行头产生与之垂直的转动从而实现另一方向上的角度调节；末端的执行舵机作为带动执行器运动的动力器件，可单独运转执行动作。

进一步的，对于上述技术方案进行优化，所述的四连杆机构包括第一主动杆、第二主动杆、第一从动杆和第二从动杆，所述第一主动杆与第二转轴垂直连接并随轴转动，所述第二主动杆与第三转轴垂直连接并随轴转动，所述第一从动杆与第一主动杆铰接且固定到第二箱体，所述第二从动杆一端与第二主动杆铰接，另一端与第一从动杆铰接。

再进一步，对机械臂的重量进行合理控制，可减轻电机的负担，便于提高电机的有效功率和整体的机械效率，因此在不降低整体强度的前提下，在机械臂上做出适当的减重设置。作为优选的方案，两根主动杆和两根从动杆均采用片状的条形杆，其中第一主动杆主承重，同时用于调节第一从动杆在纵向圆周面上的运动，第一主动杆的厚度大于另外三根杆，且第一主动杆上还设有减重槽和/或减重孔，便于降低第一电机和第二电机的负担；第二主动杆、第二从动杆主调节，用于调节第一从动杆的俯仰角度，故第二主动杆和第二从动杆采用相对较薄的条形杆。第一主动杆和第二从动杆的长度相同，第二主动杆与第一从动杆的长度相同，由此形成平行四边形结构。

进一步的，对上述技术方案进行优化，第二电机和第三电机一般采用直流有刷电机，且第二电机和第三电机在第一传动箱内相对设置，所述的第二转轴和第三转轴分别连接一个两级齿轮副并与第二电机和第三电机连接传动。两个两级齿轮副的输出齿轮的轴心重合，一个输出齿轮同轴连接第二转轴，另一个输出齿轮同轴连接第三转轴。

再进一步，由于第二电机和第三电机采用直流有刷电机，其工作转矩有限，为了满足大力矩输出，在不改变电机本身的情况下，需要对传动结构进行改进，故对上述技术方案进行优化，所述的两级齿轮副为直齿轮副，传动比大于等于三。采用直齿轮副不产生轴向力，传动稳定性好，使用寿命长；传动比大于等于三，则将电机的转矩至少放大三倍，通过齿轮副作用到第二转轴和第三转轴上的转矩能够满足第一驱动箱工作输出需求。

进一步的，减速传动机构设于第二驱动箱内，将两个直流有刷电机的动力传递至套轴机臂，套轴机臂的内层输出轴和外层输出轴实现同轴相对转动；对这部分技术方案进行细化说明，所述的减速传动机构包括两组传动齿轮组，所述传动齿轮组至少包括一个主动轮和一个从动轮，第二箱体内设有第四转轴和第五转轴，两个主动轮分别与两个直流有刷电机连接，两个从动轮分别设置在第四转轴和第五转轴上；第四转轴与内层输出轴同轴转动，第五转轴与外层输出轴同轴转动。如此即能实现一个直流有刷电机控制外层输出轴运转，另一个直流有刷电机控制内层输出轴运转。

再进一步，机械臂在执行动作时，不同的转轴处对应的转矩不同，为了对转轴处的转矩进行测量，并便于进一步的校正设置，对上述技术方案进行优化，所述的第一转轴、第二转轴、第三转轴、第四转轴和第五转轴上均设置有扭矩传感器。扭矩传感器一般采用压电式传感器，通过压触产生电信号，从而计算得出该处的转矩大小。

进一步的，套轴机臂的长度一定程度上决定整个机械臂的工作半径，故在不同的场合，对轴套机臂的长度需求有所差异，为了方便进行切换，提高零部件之间的契合度，避免降低效率，对上述技术方案进行优化，所述的外层输出轴包括头部段、延伸段和尾部段，所述的头部段与减速传动机构连接，所述的延伸段的两端分别连接头部段和尾部段，所述的尾部段连接末端传动头；所述头部段、延伸段和尾部段同步转动。

进一步的，对上述技术方案进行优化，以改变末端执行头的运动方向，所述的一级齿轮副包括同轴设置在内层输出轴上的第一主动锥齿轮和设置在末端执行头上的第一从动锥齿轮，第一主动锥齿轮和第一从动锥齿轮的锥度均为45°，如此将内层输出轴的转动实现90°的转向传递。

再进一步，对上述技术方案进行优化，所述的末端传动头包括与外层输出轴连接的固定部、与固定部连接的主体部，主体部内部中空，且主体部的侧壁上设置有用于连接末端执行头的轴孔。所述固定部包括套管和平板，套管套接在外层输出轴上，平板与套管垂直固定；主体部连接固定到平板上。

再进一步，对上述技术方案进行优化，所述的末端执行头包括与末端传动头转动连接的连接板、与连接板固定连接的执行体，所述执行体上设有执行腔，执行腔的侧壁上设有通孔，执行舵机固定于执行腔内，执行舵机的转轴从通孔中伸出。

上述内容从结构上详细说明了该机械臂的组成，本发明还公开了针对上述机器人进行自适应导航的方法，主要包括运行中的寻路步骤和碰撞后的处理步骤：

寻路步骤：

S01：通过深度摄像头对0.5米范围外进行景象识别和数据获取，形成点云数据；并将点云按法向量方向过滤，排除一切法向量方向离Z轴方向夹角10度以内的点；将剩下的点发送至机器人行进的障碍地图。以此可以将所有水平的面过滤掉，只留真正的障碍点；如果前方是低缓的斜坡，由于斜坡法向量仍然在Z轴方向10度之内，因此斜坡的点仍然会被过滤掉，而不会被当做是障碍点。

S02：通过激光雷达对0.5米范围内进行扫描探测，获得激光扫描点，并对激光扫描点进行过滤筛除后，将障碍物对应的激光扫描点以噪点形式添加至障碍地图。具体细分如下步骤：

1)将所有激光扫描点按极坐标排序；

2)依次按极坐标顺序考虑每一个点；

3)若当前点P与其极坐标相近的M个点中其中任意一个点Q的直线距离小于阈值,且从激光原点O到P的连线与P到Q的连线夹角大于阈值min且小于阈值max，则记Q为P的有效邻居，否则记为无效邻居；

4)将所有有效邻居点数量小于等于2的点舍去。

对于步骤S02，实现激光扫描点的筛除算法复杂度是O(NlogN)+O(N)＝O(NlogN)，因此几乎对导航路径的算法不产生额外计算负担，不会降低激光信息处理的帧率。

S03：根据障碍地图的坐标定位，处理器指定新的行进路径，指定机器人按照新的行进路径前进。

常规的AGV驱动电机控制方法为PI速度环控制，即通过比例(Proportional)控制与积分(Integral)控制，通过实测电机转速的反馈信息来实现对转速的精确控制。这种方法是基于线性模型分析得出的，然而实际的电机并不是纯线性的，尤其是在低转速下电机由于未建模的库伦摩擦和非线性粘性摩擦，会表现出极强的非线性。其中最典型的例子就是电机死区，即PWM占空比在某一个值之下均不能使电机运转。在AGV智能平台控制中将理想的比例积分速度环控制用于非线性电机时，有如下问题：

1)低速运动时电机运转不平顺，容易频繁启停。这是因为速度环输出的PWM值常常无法突破死区。

2)电机输出速度容易在低转速时在超调与非超调之间摇摆。这是因为按线性分析时指定的速度环自然频率w与阻尼比d是按AGV正常运转的速度选取的。但由于低速的非线性性会直接导致阻尼比大幅降低，因此容易产生震荡。

3)AGV在遇到复杂障碍时，导航模块通常会让AGV低速运转，此时如果由于传感器盲区导致AGV与未发现的障碍物发生碰撞，因为非线性导致的低阻尼比会使得通过电机电流变化而判断碰撞的方法变动不敏感甚至失效。这是因为在低速下比例与积分增益实际上严重偏低，当发生碰撞时速度环的响应很慢，因此输出的PWM指令有一个明显的时间延迟才会大幅增长，最终就体现为电流变化迟缓。

为此，针对本机器人的导航控制，提出了针对碰撞后的处理步骤。

处理步骤是基于速度环自适应增益调整的碰撞检测控制方法，包括如下步骤：

S04：对电机通过转速直接调整动态调整增益参数，使电机在低速运转的非线性问题得到解决，则电机低速输出的电流值变化趋于线性。

S05：使用一阶指数过滤器对电机电流进行滤波，得到瞬时的平均电流I_avg。

S06：在任意时刻电机的实时电流|I–I_avg|>t,则判断AGV发生了碰撞，此时立刻暂停AGV的运动。并且通过不同电机电流变化的方向，评估出碰撞点相对于AGV的位置。

S07：静止等待时间T，然后进行360度低速旋转；如果电流值恢复正常，则判断遇到的是移动障碍，此时已经离开，恢复正常行进；如果旋转时电流值升高，则判断障碍依然存在，并确认此障碍为静止障碍，此时将障碍加入到障碍地图。

S08：处理器指定机器人低速后退，重新执行步骤S02和S03。

与现有技术相比，本发明的有益效果为：

1.本发明结构简单，去除了臃肿庞大的电机设置，使得机身自重小，且成本低。

2.本发明第二驱动箱的直流有刷电机后置，电机自重不成为其他电机的负担，因此可以实现比同类协作机械臂更长的臂展和更大的负重。

3.本发明使各部位相对独立，且无论是电机还是齿轮副、减速传动机构或套轴机臂，都可以通过提升功率或传动比的变化将臂展或负载进一步提升，而几乎不提高生产成本，因此具有良好的可扩展性。

4.结合激光雷达与深度摄像头，实现不同视距角度的叠加覆盖，实现了激光雷达与深度摄像头的互补；采用O(NLogN)复杂度的激光过滤方法，有效去除激光数据中的噪点，避免对AGV导航系统的干扰。通过对电机线性模型与实际非线性性的分析，根据实际速度反馈动态地对速度环增益进行调整，实现了电机全速段的平顺与灵敏响应；在机器人发生碰撞后，将碰撞本身转化为有利信息，将碰撞点加入障碍地图，为后期导航提供更丰富的参考信息。。

附图说明

为了更清楚地说明本发明实施例的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，应当理解，以下附图仅表示出了本发明的部分实施例，因此不应看作是对范围的限定，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其它相关的附图。

图1是本发明车体和执行机构的分解结构示意图。

图2是车体的整体机构示意图。

图3是车体上导航装置的安装位置示意图。

图4是执行机构一个视角的整体结构示意图。

图5是执行机构另一个视角的整体结构示意图。

图6是底座的结构示意图。

图7是涡轮蜗杆驱动机构的结构示意图

图8是第一驱动箱的内部传动结构示意图及局部放大示意图。

图9是四连杆机构的结构示意图。

图10是第二驱动箱的内部传动结构示意图。

图11是套轴机臂的整体结构示意图。

图12是套轴机臂的分解结构示意图。

图13是末端传动头处的整体结构示意图。

图14是末端传动头的内部传动结构示意图。

图15是第二驱动箱、套轴机臂、末端传动头的传动结构示意图。

图16是常规的AGV驱动电机控制流程图。

图17是改进后的AGV驱动电机控制流程图。

图中：1-底座；101-第一电机，102-蜗杆；103-涡轮；104-第一转轴；2-第一驱动箱；201-第二电机；202-第三电机；203-第二转轴；204-第三转轴；3-四连杆机构；301-第一主动杆；302-第一从动杆；303-第二主动杆；304-第二从动杆；305-减重槽；306-减重孔；4-第二驱动箱；401-直流有刷电机；402-第五转轴；403-第四转轴；5-套轴机臂；501-外层输出轴；501a-头部段；501b-延伸段；501c-尾部段；502-内层输出轴；6-末端传动头；601-固定部；602-主体部；7-末端执行头；8-执行舵机；9-扭矩传感器；10-第一主动锥齿轮；11-第一从动锥齿轮；12车体；13-基架；1301-外缘条；1302-密集层；1303-疏散层；14-电机座；15-驱动轮；16-前轮座；16’-后轮座；17-前导向轮；17’-后导向轮；18-第一卡头；19-第二卡头；20-探测摄像头；21-探测雷达。

具体实施方式

下面结合附图及具体实施例对本发明做进一步阐释。

实施例1：

如图1、图2、图3、图4、图5所示，本实施例公开了一种自适应AGV机器人,包括车体、设于车体上的导航装置和设于车体上的执行装置。

具体地说，所述的导航装置包括处理器、探测雷达21、探测摄像头20、第一过滤器和第二过滤器，探测雷达、探测摄像头、第一过滤器和第二过滤器分别与处理器相连。

在本实施例中，探测雷达采用激光探测雷达，第一过滤器为激光雷达过滤器；探测摄像头为深度摄像头，第二过滤器为深度摄像头点云过滤器。

激光雷达按照一定的向下倾角进行安装固定，从而使其可以检测到一定高度范围的障碍；深度摄像头的安装位置是朝向机器人行进的正前方，也可以向上扬起一定角度，这将使其角度更适合于观察前上方的目标，实现更复杂的功能，例如目标跟随。激光雷达与深度摄像头各自的视角虽然有限，但结合起来可以覆盖足够的范围，有效发现障碍。

针对导航装置，如果只使用激光雷达进行导航，那么往往需要多个雷达对不同角度不同高度的障碍进行全覆盖扫描，如此严重增加了成本和功耗。深度摄像头能看到更大的高度范围内的障碍物，可以弥补激光雷达的不足，但是其自身有视距窄，正前方有盲区等问题。故本实施例结合这两种装置的优点，并且再配合电机增益自适应的碰撞检测处理方法，实现了低成本可靠的导航。

所述的执行装置包括底座1、设于底座上拥有六自由度的机械臂、设于机械臂末端的执行舵机8，所述的机械臂包括依次连接的第一驱动箱2、四连杆机构3、第二驱动箱4、套轴机臂5、末端传动头6和末端执行头7。具体地说，如图6所示，所述底座上设有相对其自身转动的第一转轴104，如图8所示，所述第一驱动箱与第一转轴的上端连接固定，所述底座上设有蜗轮蜗杆驱动组件，蜗轮蜗杆驱动组件连接第一转轴的下端并同步传动；所述第一驱动箱内设有第二转轴203和第三转轴204并分别由第二电机201和第三电机202驱动，所述四连杆机构围成平行四边形且与第二转轴和第三转轴配合传动；如图10所示，所述第二驱动箱固定在四连杆机构的上端，第二驱动箱包括第二箱体，第二箱体的后表面设置有两个直流有刷电机401，第二箱体内设有分别与两个直流有刷电机连接的减速传动机构，如图11、图12、图13所示，所述套轴机臂包括分别与减速传动机构连接传动的内层输出轴502和外层输出轴501，外层输出轴连接末端传动头并带动其同轴转动，内层输出轴伸入末端传动头内部并通过一级齿轮副与末端执行头连接传动，使末端执行头与末端传动头相对转动，执行舵机的最大扭矩为50kg·cm。第二箱体上的两个直流有刷电机均为40W的有刷直流电机，电机自身带有51：1速比的行星减速箱。

本发明以车体作为机器人的行走系统，通过车体搭载执行机构行进，并利用车体上的导航系统实现自适应导航；执行机构通过涡轮蜗杆驱动组件、第一驱动箱和第二驱动箱作为整个机械臂的动力机构，以四连杆机构、轴套机臂和末端传动头作为传动机构，以末端执行头连接末端执行器实现最终的执行动作。该协作机械臂可以广泛用于生产自动化的各项工序，例如码垛、分拣、视觉检查等。此外可以与人共同部署在一个工作区，紧密与人协作从而实现工业领域以外的自动化应用，例如冲泡咖啡，做饭，康养等。

机器人为全3D打印的PLA(polylactic acid聚乳酸)材质，具有重量轻，成本低的优点。整个机器人的结构设计都是考虑到能够采用3D打印整个机械臂来考虑的，在某些受力较大的部位，对3D零部件进行了补强设计，而在一些可以减重的部位，尽量的降低了材质的厚度等参数。

其中，关于车体部分，车体包括车架和设置在车架下方的驱动系统。

在本实施例中，所述的车架包括基架13，基架包括矩形的外缘条1301和设置于外缘条内的方格状中网，所述中网包括密集层1302和疏散层1303，所述疏散层贴合在密集层的下部，疏散层和密集层的厚度相同且均为外缘条高度的一半。

外缘条和中网均采用聚氨酯材料制成，且外缘条和中网一体成型。

车架的中心位置沿宽边方向设有差分驱动装置，差分驱动装置包括主控板和两个相对设置的方形的电机座14；在本实施例中，主控板可设置在车架上，也可设置在车架上搭载的其他控制器内，当设置在其他控制器内时，便于集成化控制管理；电机座内设有与主控板电连接的电机，电机转轴连接减速器，减速器连接到驱动轮，两个电机分别驱动一个驱动轮15，当两个电机的运动状态同步时，可实现AGV的前进或后退，当两个电机的运动状态相互不同步时，可实现AGV的转向；所述车架两个宽边的中部设置有前导向组件和后导向组件，所述前导向组件和后导向组件分别包括前轮座16和后轮座16’，前轮座上设有前导向轮17，后轮座上设有后导向轮17’。

车架上需要安装承载AGV的导航装置、协作机械臂，且各部件在车架上的布局会影响AGV的运作，为了方便自由设置各部件，对其位置进行合理的布设，本实施例对上述方案进行优化，所述电机座、前轮座和后轮座上均设置有竖直向上的连接卡头，连接卡头包括卡入疏散层的第一卡头18和卡入密集层的第二卡头19，第二卡头位于第一卡头的顶端，且第二卡头的顶端设置有螺孔8。第一卡头和第二卡头均为方形，且第二卡头的尺寸与密集层的网孔尺寸相同。

疏散层和密集层对应的网孔尺寸不同，疏散层的网孔尺寸大于密集层的网孔尺寸，增加连接固定点可提高安装的稳定性，因此对上述技术方案进行优化，所述的第一卡头顶端设有两个第二卡头或者三个卡头。

电机座相对于前轮座和后轮座来说尺寸较大，且安装驱动力部件，容易松动，因此需要设置更多的点受力进行稳定；而前轮座和后轮座的结构尺寸相对较小，仅需安装从动部件，其稳定性比较高；故本实施例对上述的技术方案进行优化，所述的电机座上的连接卡头数量大于二，且均位于电机座的侧表面，前轮座和后轮座上的连接卡头分别位于其上端面。

在本实施例中，为了提高稳定性，降低安装难度，提高安装的便捷性，电机座、前轮座和后轮座与设于其上的连接卡头一体成型。

将中网设置成密集层和疏散层，其中密集层用于连接固定，保证连接部件的稳定性，疏散层的网孔尺寸大于密集层的网孔尺寸，能够减少用料，降低车架的自重，减少能耗，增加续航。在本实施例中如下设置，疏散层一个网孔的长边方向对应密集层两个按长边排布的网孔，疏散层一个网孔的短边方向上对应密集层三个按短边排布的网孔。

疏散层和所述的密集层均包括横向格条和纵向格条，横向格条与纵向格条相互垂直，横向格条与纵向格条的厚度相同。

本实施例中，横向格条平行于外缘条的长边方向，且密集层相邻横向格条之间的间距等于横向格条的厚度。

另外，关于协作机械臂部分，所述协作机械臂的底座连接固定在车体的上表面；如图7所示，所述涡轮蜗杆驱动组件包括第一电机101，第一电机连接蜗杆102，蜗杆与设于第一转轴上的涡轮103配合传动，第一电机运转后通过蜗轮蜗杆带动第一转轴转动，从而实现机械臂在水平方向的转动；第一驱动箱内的第二转轴和第三转轴实现带动四连杆机构的运行，实现第二驱动箱在纵向上的圆周运动和俯仰动作；第二驱动箱上的直流有刷电机通过减速传动机构控制套轴机臂的内外输出轴运转，外层输出轴的转动带动末端传动头和末端执行头产生同轴转动，而内层输出轴的转动可带动末端执行头产生与之垂直的转动从而实现另一方向上的角度调节；末端的执行舵机作为带动执行器运动的动力器件，可单独运转执行动作。

第一电机、第二电机和第三电机均为90W的有刷直流电机，电机自身带有77：1速比的行星减速箱。

如图9所示，对于上述技术方案进行优化，所述的四连杆机构包括第一主动杆301、第二主动杆303、第一从动杆302和第二从动杆304，所述第一主动杆与第二转轴垂直连接并随轴转动，所述第二主动杆与第三转轴垂直连接并随轴转动，所述第一从动杆与第一主动杆铰接且固定到第二箱体，所述第二从动杆一端与第二主动杆铰接，另一端与第一从动杆铰接。

对机械臂的重量进行合理控制，可减轻电机的负担，便于提高电机的有效功率和整体的机械效率，因此在不降低整体强度的前提下，在机械臂上做出适当的减重设置。作为优选的方案，两根主动杆和两根从动杆均采用片状的条形杆，其中第一主动杆主承重，长度为50cm，同时用于调节第一从动杆在纵向圆周面上的运动，第一主动杆的厚度大于另外三根杆，且第一主动杆上还设有减重槽305和/或减重孔306，便于降低第一电机和第二电机的负担；第二主动杆、第二从动杆主调节，用于调节第一从动杆的俯仰角度，故第二主动杆和第二从动杆采用相对较薄的条形杆。第一主动杆和第二从动杆的长度相同，第二主动杆与第一从动杆的长度相同，由此形成平行四边形结构。

如图8所示，对上述技术方案进行优化，所述的第二转轴和第三转轴分别连接一个两级齿轮副并与第二电机和第三电机连接传动。两个两级齿轮副的输出齿轮的轴心重合，一个输出齿轮同轴连接第二转轴，另一个输出齿轮同轴连接第三转轴。

由于第二电机和第三电机采用直流有刷电机，其工作转矩有限，为了满足大力矩输出，在不改变电机本身的情况下，需要对传动结构进行改进，故对上述技术方案进行优化，所述的两级齿轮副为直齿轮副，传动比等于三。采用直齿轮副不产生轴向力，传动稳定性好，使用寿命长；传动比等于三，则将电机的转矩放大三倍，通过齿轮副作用到第二转轴和第三转轴上的转矩能够满足第一驱动箱工作输出需求。

如图10所示，减速传动机构设于第二驱动箱内，将两个直流有刷电机的动力传递至套轴机臂，套轴机臂的内层输出轴和外层输出轴实现同轴相对转动；对这部分技术方案进行细化说明，所述的减速传动机构包括两组传动齿轮组，所述传动齿轮组包括一个主动轮和一个从动轮，第二箱体内设有第四转轴403和第五转轴402，两个主动轮分别与两个直流有刷电机连接，两个从动轮分别设置在第四转轴和第五转轴上；第四转轴与内层输出轴同轴转动，第五转轴与外层输出轴同轴转动。如此即能实现一个直流有刷电机控制外层输出轴运转，另一个直流有刷电机控制内层输出轴运转。

机械臂在执行动作时，不同的转轴处对应的转矩不同，为了对转轴处的转矩进行测量，并便于进一步的校正设置，对上述技术方案进行优化，所述的第一转轴、第二转轴、第三转轴、第四转轴和第五转轴上均设置有扭矩传感器9。扭矩传感器采用压电式传感器，通过压触产生电信号，从而计算得出该处的转矩大小。

如图12所示，套轴机臂的长度一定程度上决定整个机械臂的工作半径，故在不同的场合，对轴套机臂的长度需求有所差异，为了方便进行切换，提高零部件之间的契合度，避免降低效率，对上述技术方案进行优化，所述的外层输出轴包括头部段501a、延伸段501b和尾部段501c，所述的头部段与减速传动机构连接，所述的延伸段的两端分别连接头部段和尾部段，所述的尾部段连接末端传动头；所述头部段、延伸段和尾部段同步转动。

如图13、图14、图15所示，对上述技术方案进行优化，以改变末端执行头的运动方向，所述的一级齿轮副包括同轴设置在内层输出轴上的第一主动锥齿轮10和设置在末端执行头上的第一从动锥齿轮11，第一主动锥齿轮和第一从动锥齿轮的锥度均为45°，如此将内层输出轴的转动实现90°的转向传递。

对上述技术方案进行优化，所述的末端传动头包括与外层输出轴连接的固定部601、与固定部连接的主体部602，主体部内部中空，且主体部的侧壁上设置有用于连接末端执行头的轴孔。所述固定部包括套管和平板，套管套接在外层输出轴上，平板与套管垂直固定；主体部连接固定到平板上。

对上述技术方案进行优化，所述的末端执行头包括与末端传动头转动连接的连接板、与连接板固定连接的执行体，所述执行体上设有执行腔，执行腔的侧壁上设有通孔，执行舵机固定于执行腔内，执行舵机的转轴从通孔中伸出。

实施例2：

本实施例公开了对实施例1中公开的机器人进行自适应导航的方法，主要包括运行中的寻路步骤和碰撞后的处理步骤：

寻路步骤：

S01：通过深度摄像头对0.5米范围外进行景象识别和数据获取，形成点云数据；并将点云按法向量方向过滤，排除一切法向量方向离Z轴方向夹角10度以内的点；将剩下的点发送至机器人行进的障碍地图。以此可以将所有水平的面过滤掉，只留真正的障碍点；如果前方是低缓的斜坡，由于斜坡法向量仍然在Z轴方向10度之内，因此斜坡的点仍然会被过滤掉，而不会被当做是障碍点。

S02：通过激光雷达对0.5米范围内进行扫描探测，获得激光扫描点，并对激光扫描点进行过滤筛除后，将障碍物对应的激光扫描点以噪点形式添加至障碍地图。具体细分如下步骤：

1)将所有激光扫描点按极坐标排序；

2)依次按极坐标顺序考虑每一个点；

3)若当前点P与其极坐标相近的M个点中其中任意一个点Q的直线距离小于阈值,且从激光原点O到P的连线与P到Q的连线夹角大于阈值min且小于阈值max，则记Q为P的有效邻居，否则记为无效邻居；

4)将所有有效邻居点数量小于等于2的点舍去。

对于步骤S02，实现激光扫描点的筛除算法复杂度是O(NlogN)+O(N)＝O(NlogN)，因此几乎对导航路径的算法不产生额外计算负担，不会降低激光信息处理的帧率。

S03：根据障碍地图的坐标定位，处理器指定新的行进路径，指定机器人按照新的行进路径前进。

如图16所示，常规的AGV驱动电机控制方法为PI速度环控制，即通过比例(Proportional)控制与积分(Integral)控制，通过实测电机转速的反馈信息来实现对转速的精确控制。

其中比例与积分控制模型为：Kv×(1+sTv)/s，其中电机线性模型为：Km/(1+sTm)。Kv是比例与积分增益的乘积；Km是电机扭矩常数；Tm是电机常数：

Tv一般取为Tm，从而消去电机模型中s＝-1/Tm的极点。

这种方法是基于线性模型分析得出的，然而实际的电机并不是纯线性的，尤其是在低转速下电机由于未建模的库伦摩擦和非线性粘性摩擦，会表现出极强的非线性。其中最典型的例子就是电机死区，即PWM占空比在某一个值之下均不能使电机运转。在AGV智能平台控制中将理想的比例积分速度环控制用于非线性电机时，有如下问题：

1)低速运动时电机运转不平顺，容易频繁启停。这是因为速度环输出的PWM值常常无法突破死区。

2)电机输出速度容易在低转速时在超调与非超调之间摇摆。这是因为按线性分析时指定的速度环自然频率w与阻尼比d是按AGV正常运转的速度选取的。但由于低速的非线性性会直接导致阻尼比大幅降低，因此容易产生震荡。

3)AGV在遇到复杂障碍时，导航模块通常会让AGV低速运转，此时如果由于传感器盲区导致AGV与未发现的障碍物发生碰撞，因为非线性导致的低阻尼比会使得通过电机电流变化而判断碰撞的方法变动不敏感甚至失效。这是因为在低速下比例与积分增益实际上严重偏低，当发生碰撞时速度环的响应很慢，因此输出的PWM指令有一个明显的时间延迟才会大幅增长，最终就体现为电流变化迟缓。

为此，针对本机器人的导航控制，提出了针对碰撞后的处理步骤。

如图17所示，根据电机的速度反馈调节Kv与Km。其中Kv的调整规则为Kv＝Kv0*(1+max(0,w_min-w)*p)。Kv0为原本线性模型的增益值,w为转速反馈,w_min为需要进行自适应调整的最低转速,p为增益调整的比例值。这样在电机正常速度运转时并不会调整增益，但当低于一个速度阈值w_min时就会按比例逐渐增加增益。

处理步骤是基于速度环自适应增益调整的碰撞检测控制方法，包括如下步骤：

S04：对电机通过转速直接调整动态调整增益参数，使电机在低速运转的非线性问题得到解决，则电机低速输出的电流值变化趋于线性。

S05：使用一阶指数过滤器对电机电流进行滤波，得到瞬时的平均电流I_avg。

S06：在任意时刻电机的实时电流|I–I_avg|>t,则判断AGV发生了碰撞，此时立刻暂停AGV的运动。并且通过不同电机电流变化的方向，评估出碰撞点相对于AGV的位置。

S07：静止等待时间T，然后进行360度低速旋转；如果电流值恢复正常，则判断遇到的是移动障碍，此时已经离开，恢复正常行进；如果旋转时电流值升高，则判断障碍依然存在，并确认此障碍为静止障碍，此时将障碍加入到障碍地图。

S08：处理器指定机器人低速后退，重新执行步骤S02和S03。

使用本实施例中公开的控制方法对机器人进行控制的好处是：

1.结合激光雷达与深度摄像头，实现不同视距角度的叠加覆盖。深度摄像头除了服务于AGV导航，本身还能被用于AGV的更高级的视觉系统中，例如进行目标跟踪。

2.采用O(NLogN)复杂度的激光过滤方法，有效去除激光数据中的噪点，避免对AGV导航系统的干扰。

3.将深度摄像头的点云数据提取出障碍点云部分，并结合激光雷达解决深度摄像头0.5米的盲区问题，实现了激光雷达与深度摄像头的互补。

4.通过对电机线性模型与实际非线性性的分析，根据实际速度反馈动态地对速度环增益进行调整，实现了电机全速段的平顺与灵敏响应，最终使得在低速下的碰撞检测成为可能。

5.在低成本的传感器解决方案下，AGV的碰撞在实际运行中可能无法避免。本方法将碰撞本身转化为有利信息，将碰撞点加入障碍地图，为后期导航提供更丰富的参考信息。

以上即为本发明列举的几种实施方式，但本发明不局限于上述可选的实施方式，本领域技术人员可根据上述方式相互任意组合得到其他多种实施方式，任何人在本发明的启示下都可得出其他各种形式的实施方式。上述具体实施方式不应理解成对本发明的保护范围的限制，本发明的保护范围应当以权利要求书中界定的为准，并且说明书可以用于解释权利要求书。

Claims (7)

1.一种自适应AGV机器人,包括车体、设于车体上的导航装置和设于车体上的执行装置；其特征在于：

所述的导航装置包括处理器、探测雷达(21)、探测摄像头(20)、第一过滤器和第二过滤器，探测雷达(21)、探测摄像头(20)、第一过滤器和第二过滤器分别与处理器相连；

执行装置包括底座(1)、设于底座上拥有六自由度的机械臂、设于机械臂末端的执行舵机(8)，所述的机械臂包括依次连接的第一驱动箱(2)、四连杆机构(3)、第二驱动箱(4)、套轴机臂(5)、末端传动头(6)和末端执行头(7)；所述底座上设有相对其自身转动的第一转轴(104)，所述第一驱动箱与第一转轴的上端连接固定，所述底座上设有蜗轮蜗杆驱动组件，蜗轮蜗杆驱动组件连接第一转轴的下端并同步传动；所述第一驱动箱内设有第二转轴(203)和第三转轴(204)并分别由第二电机(201)和第三电机(202)驱动，所述四连杆机构围成平行四边形且与第二转轴和第三转轴配合传动；所述第二驱动箱固定在四连杆机构的上端，第二驱动箱包括第二箱体，第二箱体的后表面设置有两个直流有刷电机(401)，第二箱体内设有分别与两个直流有刷电机连接的减速传动机构，所述套轴机臂包括分别与减速传动机构连接传动的内层输出轴(502)和外层输出轴(501)，外层输出轴连接末端传动头并带动其同轴转动，内层输出轴伸入末端传动头内部并通过一级齿轮副与末端执行头连接传动，使末端执行头与末端传动头相对转动；

所述的车体包括车架和设置在车架下方的驱动系统；所述的车架包括基架(13)，基架包括外缘条(1301)和设置于外缘条内的方格状中网，所述中网包括密集层(1302)和疏散层(1303)，所述疏散层贴合在密集层的下部；所述车架的中心位置沿宽边方向设有差分驱动装置，差分驱动装置包括主控板和两个相对设置的方形的电机座(14)，电机座内设有与主控板电连接的电机，电机转轴连接减速器，减速器连接到驱动轮(15)；所述车架两个宽边的中部设置有前导向组件和后导向组件，所述前导向组件和后导向组件分别包括前轮座(16)和后轮座(16’)，前轮座上设有前导向轮(17)，后轮座上设有后导向轮(17’)；所述电机座、前轮座和后轮座上均设置有连接卡头，连接卡头包括第一卡头(18)和第二卡头(19)；

所述的电机座上的连接卡头位于电机座的侧表面，前轮座和后轮座上的连接卡头分别位于其上端面；且所述的第一卡头顶端设有至少两个第二卡头；所述的疏散层和所述的密集层均包括横向格条和纵向格条，横向格条与纵向格条相互垂直，横向格条与纵向格条的厚度相同；且所述的横向格条平行于外缘条的长边方向，且密集层相邻横向格条之间的间距等于横向格条的厚度；

所述探测摄像头(20)用于进行景象识别和数据获取，形成点云数据；并将点云按法向量方向过滤，排除一切法向量方向离Z轴方向夹角10度以内的点；将剩下的点发送至机器人行进的障碍地图；

所述探测雷达(21)用于进行扫描探测，获得激光扫描点，并对激光扫描点进行过滤筛除后，将障碍物对应的激光扫描点以噪点形式添加至障碍地图；

所述第一过滤器和第二过滤器用于对第二电机(201)和第三电机(202)的电流进行滤波，得到瞬时的平均电流I_avg；

所述处理器用于根据障碍地图，指定新的行进路径，指定AGV机器人按照新的行进路径前进或后退，具体包括：

对第二电机(201)或/和第三电机(202)通过转速直接调整动态调整增益参数，使第二电机(201)或/和第三电机(202)在低速运转的非线性问题得到解决，则第二电机(201)或/和第三电机(202)低速输出的电流值变化趋于线性；

在任意时刻第二电机(201)或/和第三电机(202)的实时电流|I–I_avg|>t,则判断AGV机器人发生了碰撞，此时立刻暂停AGV机器人的运动；并且通过第二电机(201)或/和第三电机(202)电流变化的方向，评估出碰撞点相对于AGV机器人的位置；

静止等待时间T，然后控制AGV机器人进行360度低速旋转；如果电流值恢复正常，则判断遇到的是移动障碍，此时已经离开，恢复正常行进；如果旋转时电流值升高，则判断障碍依然存在，并确认此障碍为静止障碍，此时将障碍加入到障碍地图。

2.根据权利要求1所述的一种自适应AGV机器人,其特征在于：所述的四连杆机构包括第一主动杆(301)、第二主动杆(303)、第一从动杆(302)和第二从动杆(304)，所述第一主动杆与第二转轴垂直连接并随轴转动，所述第二主动杆与第三转轴垂直连接并随轴转动，所述第一从动杆与第一主动杆铰接且固定到第二箱体，所述第二从动杆一端与第二主动杆铰接，另一端与第一从动杆铰接。

3.根据权利要求1所述的一种自适应AGV机器人,其特征在于：所述的减速传动机构包括两组传动齿轮组，所述传动齿轮组至少包括一个主动轮和一个从动轮，第二箱体内设有第四转轴(403)和第五转轴(402)，两个主动轮分别与两个直流有刷电机连接，两个从动轮分别设置在第四转轴和第五转轴上；第四转轴与内层输出轴同轴转动，第五转轴与外层输出轴同轴转动。

4.根据权利要求3所述的一种自适应AGV机器人,其特征在于：所述的第一转轴、第二转轴、第三转轴、第四转轴和第五转轴上均设置有扭矩传感器(9)。

5.根据权利要求1所述的一种自适应AGV机器人,其特征在于：所述的一级齿轮副包括同轴设置在内层输出轴上的第一主动锥齿轮(10)和设置在末端执行头上的第一从动锥齿轮(11)，第一主动锥齿轮和第一从动锥齿轮的锥度均为45°。

6.根据权利要求1所述的一种自适应AGV机器人,其特征在于：所述的末端传动头包括与外层输出轴连接的固定部(601)、与固定部连接的主体部(602)，主体部内部中空，且主体部的侧壁上设置有用于连接末端执行头的轴孔。

7.用于对权利要求1-6任一项所述的机器人进行自适应导航的方法，其特征在于，包括运行中的寻路步骤和碰撞后的处理步骤：

寻路步骤：

S01：通过探测摄像头(20)对0.5米范围外进行景象识别和数据获取，形成点云数据；并将点云按法向量方向过滤，排除一切法向量方向离Z轴方向夹角10度以内的点；将剩下的点发送至机器人行进的障碍地图；以此可以将所有水平的面过滤掉，只留真正的障碍点；如果前方是低缓的斜坡，由于斜坡法向量仍然在Z轴方向10度之内，因此斜坡的点仍然会被过滤掉，而不会被当做是障碍点；

S02：通过探测雷达(21)对0.5米范围内进行扫描探测，获得激光扫描点，并对激光扫描点进行过滤筛除后，将障碍物对应的激光扫描点以噪点形式添加至障碍地图；具体细分如下步骤：

1)将所有激光扫描点按极坐标排序；

2)依次按极坐标顺序考虑每一个点；

3)若当前点P与其极坐标相近的M个点中其中任意一个点Q的直线距离小于阈值,且从激光原点O到P的连线与P到Q的连线夹角大于阈值min且小于阈值max，则记Q为P的有效邻居，否则记为无效邻居；

4)将所有有效邻居点数量小于等于2的点舍去；

S03：根据障碍地图的坐标定位，处理器指定新的行进路径，指定AGV机器人按照新的行进路径前进；

处理步骤是基于速度环自适应增益调整的碰撞检测控制方法，包括如下步骤：

S04：对第二电机(201)或/和第三电机(202)通过转速直接调整动态调整增益参数，使第二电机(201)或/和第三电机(202)在低速运转的非线性问题得到解决，则第二电机(201)或/和第三电机(202)低速输出的电流值变化趋于线性；

S05：使用第一过滤器和第二过滤器用于对第二电机(201)和第三电机(202)的电流进行滤波，得到瞬时的平均电流I_avg；

S06：在任意时刻第二电机(201)或/和第三电机(202)的实时电流|I–I_avg|>t,则判断AGV机器人发生了碰撞，此时立刻暂停AGV机器人的运动；并且通过第二电机(201)或/和第三电机(202)电流变化的方向，评估出碰撞点相对于AGV机器人的位置；

S07：静止等待时间T，然后控制AGV机器人进行360度低速旋转；如果电流值恢复正常，则判断遇到的是移动障碍，此时已经离开，恢复正常行进；如果旋转时电流值升高，则判断障碍依然存在，并确认此障碍为静止障碍，此时将障碍加入到障碍地图；

S08：处理器指定AGV机器人低速后退，重新执行步骤S02和S03。

Images