CN108563122B - 一种移动机器人速度平滑插值方法 - Google Patents

Abstract

本发明公开了一种移动机器人速度平滑插值方法，包括以下步骤，建立插值函数模型；对所述插值函数模型进行优化处理得到速度控制曲线模型；将优化处理后的所述速度控制曲线模型与所述移动机器人当前状态结合得到电机所需的控制量模型；根据不同移动平台的控制频率提前计算出能够选取的插值量。本发明的有益效果：一是使用灵活，能够根据不同平台灵活选取插值点进行控制量计算；二是能够有效提升手动操作模式下移动机器人的运动性能，避免启停剧烈造成的危险；三是对于运行速度越高的移动机器人，插值效果越明显。

Description

一种移动机器人速度平滑插值方法

技术领域

本发明涉及移动机器人的技术领域，尤其涉及一种移动机器人速度平滑插值方法。

背景技术

现有移动机器人的控制***一般分为两部分，负责数据运算的上位机和负责轮速控制的下位机，上位机经过运动学计算将控制速度发送给下位机，下位机不时读取设备状态数据，转换成数字信号反馈给上位机。而移动机器人的运动模式通常包括自主导航和手动操作两种方式；自主导航时，控制速度由上位机进行计算并发送；手动操作则是为了满足一些特殊情况，人为的操控移动机器人的运动。手动操作时，通常需要完成移动机器人的起步、停止、转弯操作。目前，多采用固定的加速度进行手动操作来实现上述三个功能。对于低惯量移动机器人，其速度基本能瞬间达到目标速度，启停剧烈造成车体抖动；然而，对于一些装载有机械手的或者惯量较大的移动机器人，上述现象就显得十分危险，因此需要采取合适的措施避免启停剧烈的现象。

目前在工业机器人领域，对速度插值的研究已经诞生多种类型的插值函数，但在移动机器人领域这一片内容尚属空白，并且现有曲线速度控制方法用于手动操作下的机械臂单轴控制，将其加速和减速过程各分为三段进行区分，最后采用线性化后的加速度进行机械臂的控制。上述操作通过对电机脉冲数计算来判断加减速过程，对移动机器人来讲，由于控制速度的发送基于车体中心坐标系，并无装置可以提供上述脉冲数。

发明内容

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

鉴于上述现有移动机器人存在的问题，提出了本发明。

因此，本发明目的是提供一种手动操作模式下的移动机器人速度插值方法，根据移动机器人的运动需求，提出采用S型插值函数进行移动机器人的手动操作速度控制，避免移动机器人手动操作模式下启停剧烈的现象。

为解决上述技术问题，本发明提供如下技术方案：一种移动机器人速度平滑插值方法，包括以下步骤，建立插值函数模型；对所述插值函数模型进行优化处理得到速度控制曲线模型；将优化处理后的所述速度控制曲线模型与所述移动机器人当前状态结合得到电机所需的控制量模型；根据不同移动平台的控制频率提前计算出能够选取的插值量，并将选取的插值量直接代入所述控制量模型中计算，将计算结果应用于移动机器人速度控制***中。

作为本发明所述的移动机器人速度平滑插值方法的一种优选方案，其中：所述建立插值函数模型为S型函数，且函数模型：

作为本发明所述的移动机器人速度平滑插值方法的一种优选方案，其中：所述优化处理还包括在一定的范围内对所述插值函数模型进行归一化，使其值域处于[0,1]，定义域处于[0,1]，并同时满足下列条件：

作为本发明所述的移动机器人速度平滑插值方法的一种优选方案，其中：采取缩放法进行归一化处理，并满足上述条件，且还包括以下步骤，

假设所述插值函数模型通过横坐标与纵坐标平移与缩放得到所需函数模型如下：

其中，参数a决定横坐标缩放尺度，k与放大倍数有关，b与相位有关，c与截距相关。

作为本发明所述的移动机器人速度平滑插值方法的一种优选方案，其中：当a∈[8,12]时曲线走势较为合理，选取a＝10作为研究对象，进行函数模型的处理。最终函数经归一化后得到速度控制曲线模型为：

作为本发明所述的移动机器人速度平滑插值方法的一种优选方案，其中：将所述速度控制曲线模型与移动机器人当前状态结合得到电机的控制量模型，包括以下步骤，

假设移动机器人当前反馈速度为v_i，对应控制量u_i，下一时刻由上位机发送来的速度为v_i+1，对应控制量u_i+1；则在下一阶段采样时间t内有，

u(t)＝u(i)+[u(i+1)-u(i)]·σ(t)

＝[1-σ(t)]·u(i)+σ(t)·u(i+1)

且如果当u(i)＝0，上式直接简化为u(t)＝σ(t)·u(i+1)。

作为本发明所述的移动机器人速度平滑插值方法的一种优选方案，其中：所述根据不同移动平台控制频率提前计算出能够选取的所述插值量，根据不同平台能够灵活变通，且该步骤中还包括将初始的插值量适当增加。

作为本发明所述的移动机器人速度平滑插值方法的一种优选方案，其中：当移动平台控制频率为4Hz时能够选取的插值量为σ(0.4)＝0.27、σ(0.5)＝0.5、σ(0.7)＝0.89、σ(1)＝1。

作为本发明所述的移动机器人速度平滑插值方法的一种优选方案，其中：对所述速度控制曲线模型进行求导，其一阶导数和二阶导数均表现出很好的连续性，对控制***非常友好。

作为本发明所述的移动机器人速度平滑插值方法的一种优选方案，其中：所述控制频率越高，速度越平滑，移动机器人的运动越平稳，且对于运行速度越高的移动机器人，插值效果越明显。

本发明的有益效果：本发明提供的一种移动机器人速度平滑插值方法，一是使用灵活，能够根据不同平台灵活选取插值点进行控制量计算；二是能够有效提升手动操作模式下移动机器人的运动性能，避免启停剧烈造成的危险；三是对于运行速度越高的移动机器人，插值效果越明显。

附图说明

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

图1为本发明S型函数曲线图；

图2为本发明S型函数曲线经过归一化处理后得到的速度控制曲线图；

图3为本发明速度控制曲线模型的一阶导数图；

图4为本发明速度控制曲线模型的二阶导数图；

图5为本发明移动机器人的控制***架构示意图。

具体实施方式

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

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

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

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

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

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

实施例1

本实施例中为了克服现有曲线速度控制方法中需要通过对电机脉冲数计算来判断加减速过程，而对移动机器人来讲，由于控制速度的发送基于车体中心坐标系，并无装置可以提供上述脉冲数的问题。因此提出一种移动机器人速度平滑插值方法，具体的，该方法包括以下步骤，

(1)建立插值函数模型；

(2)对所述插值函数模型进行优化处理得到速度控制曲线模型；

(3)将优化处理后的所述速度控制曲线模型与所述移动机器人当前状态结合得到电机所需的控制量模型；

(4)根据不同移动平台的控制频率提前计算出能够选取的插值量，并将选取的插值量直接代入所述控制量模型中计算，将计算结果应用于移动机器人速度控制***中。

进一步的，在步骤(1)中还包括根据移动机器人运动需求来选取函数模型，因为移动机器人的速度控制应满足一定的实际需求，需要结合实际来进行设计：移动机器人起步阶段要求加速度要尽可能小，且中间加速过程要十分迅速，当速度接近目标速度后，加速又要趋于平缓以保持稳定。减速阶段要求与此相似，初始减速阶段要避免减速过大，中间阶段进行快速制动，结束阶段要尽量平稳。而S型函数完全符合上述特点，且满足速度、加速度连续性的要求，因此在本步骤中选用S型函数，S型函数或者称为sigmoid函数是神经网络中常用的非线性函数，因为该函数是连续的、单调递增的数值函数，常被应用于基于BP(误差反向传播)算法的神经网络中；一般情况下，BP神经网络隐含层的传递函数是S形函数，输出层是线性函数，当然输出层也可采用S型函数，若输出层为S型函数，则输出值的范围为该S型函数的值域。本发明中则利用S形函数和其导数来进行移动机器人速度控制。参照图1所示，该S形函数模型为：

在曲线起始阶段，斜率变化平缓，满足起步平稳的要求；中间阶段，曲线上升较快，满足快速性要求；当速度接近目标速度时，曲线又趋于平缓，方便移动机器人进行速度调整。在实际应用中，上述模型还需要进一步优化才能应用于机器人的速度控制***中。

进一步的，那么本实施例中在步骤(2)中对上述S形函数进行优化处理，即归一化(单位化)处理，归一化是一种简化计算的方式，即将有量纲的表达式，经过变换，化为无量纲的表达式，成为标量。使物理***数值的绝对值变成某种相对值关系。是简化计算，缩小量值的有效办法。本步骤中采用归一化处理手段使上述函数的值域处于[0,1]，定义域处于[0,1]，并同时满足下列条件：

因此能够通过假设原函数通过横坐标与纵坐标平移与缩放，从而得到满足上述条件的目标函数，即得到速度控制曲线模型。通过上述平移和缩放处理后得到的函数模型为

其中，参数a决定横坐标缩放尺度，k与放大倍数有关，b与相位有关，c与截距相关。

根据不同a值的图像走势，发现当a∈[8,12]时曲线走势较为合理，本实施例中选取a＝10作为研究对象，进行函数模型的处理。参照图2，因此最终函数经归一化后得到速度控制曲线模型为

并且本步骤中再对上述归一化后的函数进行求导得到如图3、图4所示，分别为一阶导数(加速度)和二阶导数(加加速度)均表现出很好的连续性，因此表明经上述归一化处理后得到的速度控制曲线模型对移动机器人的速度控制***非常友好。

再进一步的，本实施例在步骤(3)中将步骤(2)得到的速度控制曲线模型与移动机器人当前状态结合得到电机的控制量模型，在将控制量模型计算出的结果应用于控制***上，具体还包括以下步骤，

假设移动机器人当前反馈速度为v_i，对应控制量u_i，下一时刻由上位机发送来的速度为v_i+1，对应控制量u_i+1；则在下一阶段采样时间t内有，

u(t)＝u(i)+[u(i+1)-u(i)]·σ(t)

＝[1-σ(t)]·u(i)+σ(t)·u(i+1)

其为最终得到的电机所需控制量的计算模型。且根据上述模型可知，如果当u(i)＝0，上式直接简化为u(t)＝σ(t)·u(i+1)。

该控制量模型的应用在移动机器人的控制***中还包括步骤(4)，具体的，本实施例在步骤(4)中，由于不同移动平台控制频率差异较大，为减少计算量可以事先根据控制频率计算好对应需要的插值量，再将已算好的插值量直接带入控制量模型中的计算，最后将计算控制量的结果应用于移动机器人速度控制***中，实现对机器人速度平滑、平稳的控制。且在本实施例中同时考虑移动机器人过小的加速度会导致起始阶段的爬行现象，因此初始的插值量还可以根据不同情况适当增加。

根据不同平台的控制频率还可以灵活变通，在本实施例中以控制频率4Hz为例，根据此控制频率事先计算出可选取的插值量有如下四个：σ(0.4)＝0.27、σ(0.5)＝0.5、σ(0.7)＝0.89、σ(1)＝1。一般情况下，控制频率越高，速度越平滑，移动机器人的运动越平稳；且对于运行速度越高的移动机器人，本实施例的插值效果越明显。

参照图5所示，本实施例中还需要说明的是，图5所示为JNPF-4WD-01型移动机器人的控制***架构示意图，其中STM32模块主要进行电机的速度闭环控制。STM32模块，采用STM32F40x系列芯片，用于接收嵌入式控制板发来的速度命令控制电机运行；进行编码器信息的采集与计算，并通过PID闭环实现电机速度的稳定控制；与嵌入式Linux平台之间通过串口进行通信。

由于不同移动机器人平台采用的电机种类有较大差别，性能也参差不齐，响应速度存在差异，高性能电机的往往具有较好的响应速度，从而可以实现更高频率的控制，一般可达20Hz以上。

本实施例中移动机器人采用无刷直流电机作为执行元件，经多次试验测试，当控制频率选为4-5Hz时移动机器人具有较好的运动表现。因此以速度插值频率选为4Hz举例说明，那么试验优选出能够选取的插值量为σ(0.4)＝0.27、σ(0.5)＝0.5、σ(0.7)＝0.89、σ(1)＝1机器人的运动最佳。

目前已有相关技术是采用S型函数进行机械臂的速度控制优化，将机械臂的加速阶段分为加加速阶段、匀加速阶段和减加速阶段，不同阶段采用不同的加速度限制进行速度控制，根据编码器脉冲数确定所处阶段。

本实施例中由于移动机器人的电机控制能够实现PID速度闭环，因此只需要将当前控制周期应达到的目标速度并发送给下位机，即可实现速度控制的目标，速度的闭环由STM32实现。上述过程无需关心移动机器人的加速度限制问题，以及解决移动机器人无法基于车体中心坐标系安装相应的编码器来控制速度的问题，而本实施例中编码器是通过控制***STM32板来实现编码器的功能，因此只需要根据插值函数计算好在控制周期内应达到的目标量即可。

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

Claims (4)

1.一种移动机器人速度平滑插值方法，其特征在于：包括以下步骤，

建立插值函数模型；

对所述插值函数模型进行优化处理得到速度控制曲线模型；

将优化处理后的所述速度控制曲线模型与所述移动机器人当前状态结合得到电机所需的控制量模型；

根据不同移动平台的控制频率提前计算出能够选取的插值量，并将选取的插值量直接代入所述控制量模型中计算，将计算结果应用于移动机器人速度控制***中；

所述建立插值函数模型为S型函数，且函数模型：

所述优化处理还包括在一定的范围内对所述插值函数模型进行归一化，使其值域处于[0,1]，定义域处于[0,1]，并同时满足下列条件：

假设所述插值函数模型通过横坐标与纵坐标平移与缩放得到所需函数模型如下：

其中，参数a决定横坐标缩放尺度，k与放大倍数有关，b与相位有关，c与截距相关，x为横坐标；

当a∈[8,12]时曲线走势较为合理，选取a＝10作为研究对象，函数经归一化后得到速度控制曲线模型为：

2.如权利要求1所述的移动机器人速度平滑插值方法，其特征在于：将所述速度控制曲线模型与移动机器人当前状态结合得到电机的控制量模型，包括以下步骤，

假设移动机器人当前反馈速度为v_i，对应控制量u_i，下一时刻由上位机发送来的速度为v_i+1，对应控制量u_i+1；则在下一阶段采样时间t内有，

u(t)＝u(i)+[u(i+1)-u(i)]·σ(t)

＝[1-σ(t)]·u(i)+σ(t)·u(i+1)

且如果当u(i)＝0，上式直接简化为u(t)＝σ(t)·u(i+1)。

3.如权利要求1或2所述的移动机器人速度平滑插值方法，其特征在于：所述根据不同移动平台控制频率提前计算出能够选取的所述插值量，根据不同平台能够灵活变通。

4.如权利要求3所述的移动机器人速度平滑插值方法，其特征在于：当移动平台控制频率为4Hz时能够选取的插值量为σ(0.4)＝0.27、σ(0.5)＝0.5、σ(0.7)＝0.89、σ(1)＝1。

Images