CN116736866A - 一种物业室内机器人路径规划与轨迹控制方法及系统 - Google Patents

Abstract

本发明公开了一种物业室内机器人路径规划与轨迹控制方法及系统，涉及物业室内服务机器人技术领域，包括：规划初步路径，通过PRM算法在学习阶段预处理地图采样节点，构建概率路径图，利用Dijkstra搜索算法在查询阶段在连接完成的路径图中进行检索，选用距离作为优化指标，查询从起始位置到达目标位置的最短路径；获取期望轨迹与实际车辆的位置坐标之间的关系，利用PID方法计算与期望位置的差值得出当前位置，并调整自身运动速度，控制车辆的前轮转角；通过贝塞尔曲线原理对生成的轨迹进行平滑处理。本发明提供的物业室内机器人路径规划与轨迹控制方法通过PID方法控制车辆的前轮转角实现对车辆的横向控制。

Description

一种物业室内机器人路径规划与轨迹控制方法及系统

技术领域

本发明涉及物业室内服务机器人技术领域，具体为一种物业室内机器人路径规划与轨迹控制方法及系统。

背景技术

近年来，随着智能机器人技术的快速发展，服务机器人行业也得到了快速发展。服务机器人具有智能化、自动化、高效率、低成本等特点，可以用于各种行业，如旅游、教育、医疗、财务、娱乐等，为人类带来极大的便利。

但是现有服务机器人的发展在能源、电机、人机交互、感知、导航、控制等诸多方面面临许多的问题。在硬件方面，虽然能较好的完成路径的行驶，但对于硬件设备的需求较高，精细化机电系统和相关操控技术以及服务机器人的整体设计有很大的进步空间。在软件方面，不同服务机器人对于路径规划与轨迹控制的算法略有差异，复杂算法为了保障控制精度对于算力、对于软件方面要求较高。因此，对于机器人持高速增长的态势，仍有较大的发展空间。

现有的物业室内机器人的工作环境较为复杂包含了地面包含水分的环境和光滑的嗲面，当机器人处于这种行驶环境是时存在运动控制问题，尤其是横向运动极不稳定。目前的路径规划算法是车体轮廓约束、运动学/动力学约束、障碍物约束、最小运行时间等约束下的点到点路径及轨迹规划，并不容易处理复杂环境，且算法仅仅适用于特地给机器人型号。

因此，亟需一种物业室内机器人路径规划与轨迹控制方法。

发明内容

本部分的目的在于概述本发明的实施例的一些方面以及简要介绍一些较佳实施例。在本部分以及本申请的说明书摘要和发明名称中可能会做些简化或省略以避免使本部分、说明书摘要和发明名称的目的模糊，而这种简化或省略不能用于限制本发明的范围。

鉴于上述存在的问题，提出了本发明。

因此，本发明解决的技术问题是：现有的室内机器人路径规划方法存在适用性差，路径规划方法仅适用于特定的机器人，以及如何在有水迹的地面稳定行驶的优化问题。

为解决上述技术问题，本发明提供如下技术方案：一种物业室内机器人路径规划与轨迹控制方法，包括：规划初步路径，通过PRM算法在学习阶段预处理地图采样节点，构建概率路径图，利用Dijkstra搜索算法在查询阶段在连接完成的路径图中进行检索，选用距离作为优化指标，查询从起始位置到达目标位置的最短路径；获取期望轨迹与实际车辆的位置坐标之间的关系，利用PID方法计算与期望位置的差值得出当前位置，并调整自身运动速度，控制车辆的前轮转角；通过贝塞尔曲线原理对生成的轨迹进行平滑处理。

作为本发明所述的物业室内机器人路径规划与轨迹控制方法的一种优选方案，其中：所述学习阶段包括通过PRM算法对地图中的障碍物部分和道路部分分别用黑色和白色进行表示，构建地图节点集，筛选出路径节点集，将每个节点与邻域范围内的节点进行连线，若连线不与障碍物重叠，则将连线保留，若连线路径存在障碍物则删除连线，构造概率路径图。

作为本发明所述的物业室内机器人路径规划与轨迹控制方法的一种优选方案，其中：所述查询阶段包括计算从源点至非源点节点之间的最短距离，将序号为i和j的节点分别标记为和/>，/>代表源点，/>表示源点到/>的距离，/>表示/>到/>的路径，/>表示节点之间的距离，对节点分组，将源点/>设为一组，剩余节点设为二组，计算源点至剩余节点的最短路径，表示为：

；

筛选的最小值/>，确定由于/>到/>的最短路径，将/>放入第一组，记录最短路径，计算路径/>长度的最小值，并与已录入一组的/>比较，若计算的出的长度小于/>则更新/>，若计算的出的长度大于/>则不更新。

作为本发明所述的物业室内机器人路径规划与轨迹控制方法的一种优选方案，其中：所述利用PID方法计算与期望位置的差值得出当前位置包括通过比例－积分控制消除静态误差，表示为：

；

其中，表示控制器输出的控制量；/>表示偏差信号，/>表示比例系数，为积分时间常数。

作为本发明所述的物业室内机器人路径规划与轨迹控制方法的一种优选方案，其中：所述利用PID方法计算与期望位置的差值得出当前位置还包括通过比例－微分控制消除系统震荡，表示为：

；

其中，表示微分时间常数。

作为本发明所述的物业室内机器人路径规划与轨迹控制方法的一种优选方案，其中：所述平滑处理包括在轨迹折线的每个折线段上取中点提取每个折线的起始节点、中点和截止节点构建控制点集输入，控制点集表示为：

；

其中，表示折线的起始节点，/>表示折线的中点，/>表示折线的截止节点。

作为本发明所述的物业室内机器人路径规划与轨迹控制方法的一种优选方案，其中：所述平滑处理还包括将控制点集输入二阶贝塞尔曲线模型中，表示为：

；

其中，表示比例，经过平滑处理后拼接曲线，得到轨迹曲线。

本发明的另外一个目的是提供一种物业室内机器人路径规划与轨迹控制系统，其能通过PID控制系统对机器人的行驶路线进行横向调节，解决了目前的机器人行驶在含有水迹的地面不稳定的问题。

作为本发明所述的物业室内机器人路径规划与轨迹控制系统的一种优选方案，其中：包括路径规划模块，PID控制模块，平滑处理模块；所述路径规划模块内置PRM算法和Dijkstra搜索算法用于创建节点，连接节点并筛选出距离最短路径；所述PID控制模块用于消除这个静态误差并抑制超调现象，调节机器人的速度和转向，实现对机器人的横向行驶控制；所述平滑处理模块用于对机器人行驶轨迹进行平滑处理，确保机器人稳定行驶。

一种计算机设备，包括存储器和处理器，所述存储器存储有计算机程序，其特征在于，所述处理器执行所述计算机程序是实现物业室内机器人路径规划与轨迹控制方法的步骤。

一种计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现物业室内机器人路径规划与轨迹控制方法的步骤。

本发明的有益效果：本发明提供的物业室内机器人路径规划与轨迹控制方法通过PID方法控制车辆的前轮转角实现对车辆的横向控制，可以显著的提升车辆在复杂的地面横向移动的稳定性和行驶效率。通过Dijkstra搜索算法确定无障碍物最短行驶路线，避免路线规划受到过多的约束，使路径规划方法的有更好适用性。本发明在行驶稳定性、行驶效率和规划线路距离方面都取得更加良好的效果。

附图说明

为了更清楚地说明本发明实施例的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其它的附图。其中：

图1为本发明第一个实施例提供的一种物业室内机器人路径规划与轨迹控制方法的整体流程图。

图2为本发明第一个实施例提供的一种物业室内机器人路径规划与轨迹控制方法的Dijkstra算法实例图。

图3为本发明第一个实施例提供的一种物业室内机器人路径规划与轨迹控制方法的P控制效果示意图。

图4为本发明第一个实施例提供的一种物业室内机器人路径规划与轨迹控制方法的PI控制效果示意图。

图5为本发明第一个实施例提供的一种物业室内机器人路径规划与轨迹控制方法的贝塞尔曲线生成图。

图6为本发明第二个实施例提供的一种物业室内机器人路径规划与轨迹控制方法的全局地图示意图。

图7为本发明第二个实施例提供的一种物业室内机器人路径规划与轨迹控制方法的Dijkstra搜索轨迹图。

图8为本发明第二个实施例提供的一种物业室内机器人路径规划与轨迹控制方法的轨迹生成横向误差折线图。

图9为本发明第二个实施例提供的一种物业室内机器人路径规划与轨迹控制方法的车速以及速度差值折线图。

图10为本发明第二个实施例提供的一种物业室内机器人路径规划与轨迹控制方法的轨迹控制横向误差折线图。

图11为本发明第三个实施例提供的一种物业室内机器人路径规划与轨迹控制系统的整体流程图。

具体实施方式

为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合说明书附图对本发明的具体实施方式做详细的说明，显然所描述的实施例是本发明的一部分实施例，而不是全部实施例。基于本发明中的实施例，本领域普通人员在没有做出创造性劳动前提下所获得的所有其他实施例，都应当属于本发明的保护的范围。

在下面的描述中阐述了很多具体细节以便于充分理解本发明，但是本发明还可以采用其他不同于在此描述的其它方式来实施，本领域技术人员可以在不违背本发明内涵的情况下做类似推广，因此本发明不受下面公开的具体实施例的限制。

其次，此处所称的“一个实施例”或“实施例”是指可包含于本发明至少一个实现方式中的特定特征、结构或特性。在本说明书中不同地方出现的“在一个实施例中”并非均指同一个实施例，也不是单独的或选择性的与其他实施例互相排斥的实施例。

本发明结合示意图进行详细描述，在详述本发明实施例时，为便于说明，表示器件结构的剖面图会不依一般比例作局部放大，而且所述示意图只是示例，其在此不应限制本发明保护的范围。此外，在实际制作中应包含长度、宽度及深度的三维空间尺寸。

同时在本发明的描述中，需要说明的是，术语中的“上、下、内和外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。此外，术语“第一、第二或第三”仅用于描述目的，而不能理解为指示或暗示相对重要性。

本发明中除非另有明确的规定和限定，术语“安装、相连、连接”应做广义理解，例如：可以是固定连接、可拆卸连接或一体式连接；同样可以是机械连接、电连接或直接连接，也可以通过中间媒介间接相连，也可以是两个元件内部的连通。对于本领域的普通技术人员而言，可以具体情况理解上述术语在本发明中的具体含义。

实施例1

参照图1-图5，为本发明的一个实施例，提供了一种物业室内机器人路径规划与轨迹控制方法，包括：

S1：规划初步路径，通过PRM算法在学习阶段预处理地图采样节点，构建概率路径图，利用Dijkstra搜索算法在查询阶段在连接完成的路径图中进行检索，选用距离作为优化指标，查询从起始位置到达目标位置的最短路径。

更进一步的，学习阶段包括通过PRM算法对地图中的障碍物部分和道路部分分别用黑色和白色进行表示，构建地图节点集，筛选出路径节点集，将每个节点与邻域范围内的节点进行连线，若连线不与障碍物重叠，则将连线保留，若连线路径纯在障碍物则删除连线，构造概率路径图。

应说明的是，查询阶段包括计算从源点至非源点节点之间的最短距离，将序号为i和j的节点分别标记为和/>，/>代表源点，/>表示源点到/>的距离，/>表示到/>的路径，/>表示节点之间的距离，对节点分组，将源点/>设为一组，剩余节点设为二组，计算源点至剩余节点的最短路径，表示为：

；

筛选的最小值/>，确定由于/>到/>的最短路径，将/>放入第一组，记录最短路径，计算路径/>长度的最小值，并与已录入一组的/>比较，若计算的出的长度小于/>则更新/>，若计算的出的长度大于/>则不更新。

还应说明的是，如图2所示为本实施例中的一个选择最短路线的规划方法，可以筛选出最优的距离最短的路线。在算法开始之前，首先将源节点放入第一组，将剩余的节点放入第二组中。这里可以认为存在一个最短路径：/>，且/>。

查询出源点至其他每一个节点之间的最短路径，可得：

；

上述中是其中最小的值，使得/>到/>的路程进一步缩短。此时由于/>到的最短路径已经得出，所以将/>放入第一组，同时需要对最短路径进行记录。

在得出至/>的最短路径后，对路径/>长度进行计算，并将其与之前得到/>进行比较，若计算得出的长度小于/>，则更新/>，否则/>保持不变。经过上述步骤之后得到：

；

此时是其中的最小值，使得/>到/>的路程进一步缩短。此时将/>放入第一组。然后对/>长度进行计算，得到：

；

经过Dijkstra路径搜索之后的结果，得到从源点至其他点之间的最短距离。

S2：获取期望轨迹与实际车辆的位置坐标之间的关系，利用PID方法计算与期望位置的差值得出当前位置，并调整自身运动速度，控制车辆的前轮转角。

更进一步的，在实际控制功能工作中，希望车辆能够快速而且无抖动地到达100m的位置。如果控制中为了速度，全程用加速运动，那么在到达终点线附近时可能无法准确地停下来；如果控制中为了准确度，在直线上慢吞吞地运动，那么即便车辆精确地到达了100m的位置，也没有实现快速到达终点的目标，造成行驶效率低下。

应说明的是，基于P控制，如图3所示，单独使用比例项（P）进行控制，随着P值增大，实际位置快速贴近期望位置（100m处），曲线收敛位置逐渐提前，说明比例控制能够迅速反应误差，提高系统的动态响应速度，并且随着Kp的增大，响应幅度越来越大，会出现一定的超调。但是无论P项如何增大，实际位置也不能真正达到期望位置，这种现象被称为静态误差。由于P参数的作用与误差成比例，当误差减小到某个值时，P参数与误差相乘输出的结果已经小到可以忽略的地步，无法对系统进行调节，进而导致输出结果收敛至距离期望值一定距离的位置，形成静态误差。

因此，仅使用比例项P调节的结果就是，尽管P参数可以快速感受误差并进行调节，但是由于存在静态误差，实际值始终不能达到期望值，若进一步调大P参数，将导致系统出现震荡发散。因此需要积分参数I的参与，消除这个静态误差。

还应说明的是，利用PID方法计算与期望位置的差值得出当前位置包括通过比例-积分控制消除静态误差，表示为：

；

其中，表示控制器输出的控制量；/>表示偏差信号，/>表示比例系数，为积分时间常数。

离散形式模型表示为：

；

更进一步的，如图4所示加入积分控制后能够减小并消除稳态误差（曲线上移至100处）；另一方面，随之积分系数的增大，超调也随之增大（曲线两侧面积增大）。积分控制可以消除静态误差，因此称为无差调节。其控制原理为：积分项对误差进行时间积分，随着时间的增加，积分项会增大。这样，即使误差很小，积分项也会随着积分时间长度增加而变大，这使得控制器的输出增大，使稳态误差进一步减小，直到实际值收敛至期望值为止。

尽管积分项可以有效解决静态误差的问题，但是曲线的波动趋势随积分系数I的增大而增加，即超调现象越来越明显，甚至导致系统出现震荡，无法稳定收敛至期望位置。因此我们需要引入微分系数D，在系统超调处减小控制器输出，从而抑制超调现象。

应说明的是，利用PID方法计算与期望位置的差值得出当前位置还包括通过比例-微分控制消除系统震荡，表示为：

；

其中，表示微分时间常数。

离散形式模型表示为：

；

单位时间内当前时刻误差值与上一时刻误差值的差值可以认为是误差值的变化率，而微分系数D与误差变化率的乘积则为D项输出值。此外，微分系数D具有对高频噪声敏感的特性，若D值过大将放大高频噪声，进而导致D项输出值过大，造成系统震荡。

S3：通过贝塞尔曲线原理对生成的轨迹进行平滑处理。

更进一步的，初始路径规划得出的轨迹为下图5所示的折线轨迹，直接进行路径跟踪会在行驶时产生异常转弯，所以需要进行轨迹平滑后进行跟踪控制。

应说明的是，平滑处理包括在轨迹折线的每个折线段上取中点提取每个折线的起始节点、中点和截止节点构建控制点集输入，控制点集表示为：

；

其中，表示折线的起始节点，/>表示折线的中点，/>表示折线的截止节点。

还应说明的是，在连线上设置一个匀速运动的点，/>连线上设置另一个匀速运动的点，两动点连线上设置一个占动线段比率匀速增长的点，这个点的轨迹即为贝塞尔曲线，其中三个动点在所在线段上的位置比率增速相同。通过此原理可以推导出二阶贝塞尔曲线由/>坐标以及比率/>(0</><1)描述的曲线公式，平滑处理还包括将控制点集输入二阶贝塞尔曲线模型中，表示为：

；

其中，表示比例，经过平滑处理后拼接曲线，得到轨迹曲线。

实施例2

参照图6-图10，为本发明的一个实施例，提供了一种物业室内机器人路径规划与轨迹控制方法，为了验证本发明的有益效果，通过经济效益计算和仿真实验进行科学论证。

在考虑车体轮廓约束、运动学/动力学约束、障碍物约束、最小运行时间等约束下，要求自动驾驶机器人点到点路径及轨迹规划最优；与此同时要求横纵向车体运动控制功能开发，并解决机器人在湿滑地面上的运动控制问题的基础下，本实施例进行相应的工况测试：选取实时的地下停车场的工作环境为背景，在该场景下，设置相关工况进行实验。通过PRM算法搜索轨迹图后，再通过搜索算法搜索出连接起点和终点的最短路径。搜索算法采用Dijkstra算法。

如图6所示，车辆纵向控制通过给定固定车速，有车辆底层控制器进行控制，横向控制通过PID计算出前轮转角后，对全局轨迹进行跟踪，为了验证PID控制的效果，选取L形道路，对控制效果进行验证。同时对地图的创建以及全局轨迹的生成进行验证，通过ORB_SLAM在场景中创建地图。图中红色轨迹为使用PRM搜搜阶段生成的全局概率路线图。之后，采用Dijkstra算法在概率图基础上搜索出的全局轨迹，如图7所示。由图7中可以看出搜索出的轨迹对原始路径中不合理的路径点进行的去除。

全局轨迹生成后，利用轨迹跟踪算法对全局轨迹进行跟踪，并对跟踪过程中的目标点和当前位置的横向误差进行记录，横向误差结果如图8所示，跟踪过程中的横向误差最大值绝对值为0.04m，满足轨迹跟踪要求。在实验车工控机上测试得到，算法运行时延约40ms，运行频率为25Hz，满足运动控制频率25Hz要求。

之后，设计湿滑路面工况，对车辆在湿滑路面工况下的行驶性能进行测试。为了对湿滑路面的车道保持和横向控制进行测试，选取“L”型路段进行采集，地图包含直线行驶和直角弯行驶两种工况，在地图前半段，为非湿滑路面，后半段以及弯道处采用湿滑路面，地图长宽为16*3m。

为验证车辆横向和纵向控制是否符合指标要求，对车速以及横向误差进行记录。在车辆行驶过程中，给定期望车速0.5m/s，测试结果如图9和图10所示。由图可知，期望车速与实际车速差值在±0.1m/s内，横向偏差在±0.15m，满足车辆行驶要求。且车辆在起步过程以及行驶过程中未发生明显侧滑现象。

实施例3

参照图11，为本发明的一个实施例，提供了一种物业室内机器人路径规划与轨迹控制系统，包括路径规划模块，PID控制模块，平滑处理模块。

其中路径规划模块内置PRM算法和Dijkstra搜索算法用于创建节点，连接节点并筛选出距离最短路径；PID控制模块用于消除这个静态误差并抑制超调现象，调节机器人的速度和转向，实现对机器人的横向行驶控制；平滑处理模块用于对机器人行驶轨迹进行平滑处理，确保机器人稳定行驶。

功能如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备（可以是个人计算机，服务器，或者网络设备等）执行本发明各个实施例方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器（ROM，Read-OnlyMemory）、随机存取存储器（RAM，Random Access Memory）、磁碟或者光盘等各种可以存储程序代码的介质。

在流程图中表示或在此以其他方式描述的逻辑和/或步骤，例如，可以被认为是用于实现逻辑功能的可执行指令的定序列表，可以具体实现在任何计算机可读介质中，以供指令执行系统、装置或设备（如基于计算机的系统、包括处理器的系统或其他可以从指令执行系统、装置或设备取指令并执行指令的系统）使用，或结合这些指令执行系统、装置或设备而使用。就本说明书而言，“计算机可读介质”可以是任何可以包含、存储、通信、传播或传输程序以供指令执行系统、装置或设备或结合这些指令执行系统、装置或设备而使用的装置。

计算机可读介质的更具体的示例（非穷尽性列表）包括以下：具有一个或多个布线的电连接部（电子装置）、便携式计算机盘盒（磁装置）、随机存取存储器（RAM）、只读存储器（ROM）、可擦除可编辑只读存储器（EPROM或闪速存储器）、光纤装置以及便携式光盘只读存储器（CDROM）。另外，计算机可读介质甚至可以是可在其上打印程序的纸或其他合适的介质，因为可以例如通过对纸或其他介质进行光学扫描，接着进行编辑、解译或必要时以其他合适方式进行处理来以电子方式获得程序，然后将其存储在计算机存储器中。

应当理解，本发明的各部分可以用硬件、软件、固件或它们的组合来实现。在上述实施方式中，多个步骤或方法可以用存储在存储器中且由合适的指令执行系统执行的软件或固件来实现。例如，如果用硬件来实现，和在另一实施方式中一样，可用本领域公知的下列技术中的任一项或他们的组合来实现：具有用于对数据信号实现逻辑功能的逻辑门电路的离散逻辑电路，具有合适的组合逻辑门电路的专用集成电路，可编程门阵列（PGA），现场可编程门阵列（FPGA）等。应说明的是，以上实施例仅用以说明本发明的技术方案而非限制，尽管参照较佳实施例对本发明进行了详细说明，本领域的普通技术人员应当理解，可以对本发明的技术方案进行修改或者等同替换，而不脱离本发明技术方案的精神和范围，其均应涵盖在本发明的权利要求范围当中。

应说明的是，以上实施例仅用以说明本发明的技术方案而非限制，尽管参照较佳实施例对本发明进行了详细说明，本领域的普通技术人员应当理解，可以对本发明的技术方案进行修改或者等同替换，而不脱离本发明技术方案的精神和范围，其均应涵盖在本发明的权利要求范围当中。

Claims (10)

1.一种物业室内机器人路径规划与轨迹控制方法，其特征在于，包括，

规划初步路径，通过PRM算法在学习阶段预处理地图采样节点，构建概率路径图，利用Dijkstra搜索算法在查询阶段在连接完成的路径图中进行检索，选用距离作为优化指标，查询从起始位置到达目标位置的最短路径；

获取期望轨迹与实际车辆的位置坐标之间的关系，利用PID方法计算与期望位置的差值得出当前位置，并调整自身运动速度，控制车辆的前轮转角；

通过贝塞尔曲线原理对生成的轨迹进行平滑处理。

2.如权利要求1所述的物业室内机器人路径规划与轨迹控制方法，其特征在于：所述学习阶段包括通过PRM算法对地图中的障碍物部分和道路部分分别用黑色和白色进行表示，构建地图节点集，筛选出路径节点集，将每个节点与邻域范围内的节点进行连线，若连线不与障碍物重叠，则将连线保留，若连线路径存在障碍物则删除连线，构造概率路径图。

3.如权利要求1或2所述的物业室内机器人路径规划与轨迹控制方法，其特征在于：所述查询阶段包括计算从源点至非源点节点之间的最短距离，将序号为i和j的节点分别标记为 和/>，/>代表源点，/>表示源点到/>的距离，/>表示/>到/>的路径，表示节点之间的距离，对节点分组，将源点/>设为一组，剩余节点设为二组，计算源点至剩余节点的最短路径，表示为：

；

筛选的最小值/>，确定由于/>到/>的最短路径，将/>放入第一组，记录最短路径，计算路径/>长度的最小值，并与已录入一组的/>比较，若计算得出的长度小于/>则更新/>，若计算得出的长度大于/>则不更新。

4.如权利要求3所述的物业室内机器人路径规划与轨迹控制方法，其特征在于：所述利用PID方法计算与期望位置的差值得出当前位置包括通过比例-积分控制消除静态误差，表示为：

；

其中，表示控制器输出的控制量；/>表示偏差信号，/>表示比例系数，/>为积分时间常数。

5.如权利要求4所述的物业室内机器人路径规划与轨迹控制方法，其特征在于：所述利用PID方法计算与期望位置的差值得出当前位置还包括通过比例-微分控制消除系统震荡，表示为：

；

其中，表示微分时间常数。

6.如权利要求5所述的物业室内机器人路径规划与轨迹控制方法，其特征在于：所述平滑处理包括在轨迹折线的每个折线段上取中点提取每个折线的起始节点、中点和截止节点构建控制点集输入，控制点集表示为：

；

其中，表示折线的起始节点，/>表示折线的中点，/>表示折线的截止节点。

7.如权利要求6所述的物业室内机器人路径规划与轨迹控制方法，其特征在于：所述平滑处理还包括将控制点集输入二阶贝塞尔曲线模型中，表示为：

；

其中，表示比例，经过平滑处理后拼接曲线，得到轨迹曲线。

8.一种采用如权利要求1~7任一所述的物业室内机器人路径规划与轨迹控制方法的系统，其特征在于：包括路径规划模块，PID控制模块，平滑处理模块；

所述路径规划模块内置PRM算法和Dijkstra搜索算法用于创建节点，连接节点并筛选出距离最短路径；

所述PID控制模块用于消除这个静态误差并抑制超调现象，调节机器人的速度和转向，实现对机器人的横向行驶控制；

所述平滑处理模块用于对机器人行驶轨迹进行平滑处理，确保机器人稳定行驶。

9.一种计算机设备，包括存储器和处理器，所述存储器存储有计算机程序，其特征在于，所述处理器执行所述计算机程序时实现权利要求1至7中任一项所述的物业室内机器人路径规划与轨迹控制方法的步骤。

10.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现权利要求1至7中任一项所述的物业室内机器人路径规划与轨迹控制方法的步骤。