CN110794764A - 一种机器人的运动控制方法 - Google Patents

Abstract

本发明公开了一种机器人的运动控制方法，包括：步骤一、通过多个测距传感器监测所述机器人运行过程中是否有障碍物，当其中任一所述测距传感器监测到所述机器人在运行过程中有障碍物时，采集所述机器人的运行速度，所述机器人与障碍物之间的距离，多个舵机转速；步骤二、根据所述运行速度、所述距离和所述舵机转速对所述舵机的调节系数进行控制；步骤三、根据所述调节系数对所述机器人是否发生碰撞进行判断；步骤四、当判断所述机器人发生碰撞时，对机器人发出停止信号或者避障转弯信号，当判断所述机器人不发生碰撞时，对机器人发出继续前进信号。

Description

一种机器人的运动控制方法

技术领域

本发明涉及机器人避障技术领域，尤其涉及一种机器人的运动控制方法。

背景技术

机器人是由计算机控制的复杂机器，它具有类似人的肢体及感官功能； 动作程序灵活；有一定程度的智能；在工作时可以不依赖人的操纵。机器人 传感器在机器人的控制中起了非常重要的作用，正因为有了传感器，机器人 才具备了类似人类的知觉功能和反应能力。为了检测作业对象及环境或机器 人与它们的关系，在机器人上安装了触觉传感器、视觉传感器、力觉传感器、 接近觉传感器、超声波传感器和听觉传感器等，大大改善了机器人工作状况， 使其能够更充分地完成复杂的工作。由于外部传感器为集多种学科于一身的产品，有些方面还在探索之中，随着外部传感器的进一步完善，机器人的功 能越来越强大，将在许多领域为人类做出更大贡献，更好的控制机器人行驶 路线，尤其在安全领域的控制也是研究的方向之一。

发明内容

本发明设计开发了一种机器人的运动控制方法，本发明的发明目的之一 是通过对机器人的运行速度监测和与障碍物之间的距离监测等，通过对舵机 转速的调节，解决机器人在运行过程中的避障问题。

本发明提供的技术方案为：

一种机器人的运动控制方法，包括如下步骤：

步骤一、通过多个测距传感器监测所述机器人运行过程中是否有障碍物， 当其中任一所述测距传感器监测到所述机器人在运行过程中有障碍物时，采 集所述机器人的运行速度，所述机器人与障碍物之间的距离，多个舵机转速；

步骤二、根据所述运行速度、所述距离和所述舵机转速对所述舵机的调 节系数进行控制；

步骤三、根据所述调节系数对所述机器人是否发生碰撞进行判断；

步骤四、当判断所述机器人发生碰撞时，对机器人发出停止信号或者避 障转弯信号，当判断所述机器人不发生碰撞时，对机器人发出继续前进信号。

优选的是，在所述步骤一中，所述舵机包括：

第一舵机，其安装在所述机器人顶部；

第二舵机，其安装在所述机器人中心位置；

第三舵机，其安装在所述机器人距离底部10cm～25cm处；以及

所述测距传感器包括：

第一测距传感器，其安装在所述第一舵机的输出轴上，由所述第一舵机 控制所述第一测距传感器的水平转角，使所述第一测距传感器能够沿水平方 向监测与障碍物的距离；

第二测距传感器，其安装在所述第二舵机的输出轴上，由所述第二舵机 控制所述第二测距传感器的水平转角，使所述第二测距传感器能够沿水平方 向监测与障碍物的距离；

第三测距传感器，其安装在所述第三舵机的输出轴上，由所述第三舵机 控制所述第三测距传感器的水平转角，使所述第三测距传感器能够沿水平方 向监测与障碍物的距离；

其中，所述第一测距传感器、所述第二测距传感器和所述第三测距传感 器初始运行过程中，测距方向依次相差30°～45°。

优选的是，在所述步骤二中，基于BP神经网络对所述第一舵机、所述 第二舵机和所述第三舵机的调节系数进行控制，包括如下步骤：

步骤1、按照采样周期，采集所述机器人的运行速度V，采集所述机器人 与障碍物之间的距离D，采集第一舵机转速ω_a、采集第二舵机转速ω_b和采集 第三舵机转速ω_c；

步骤2、依次将上述参数进行规格化，确定三层BP神经网络的输入层向 量x＝{x₁,x₂,x₃,x₄,x₅}；其中，x₁为运行速度系数、x₂为距离系数、x₃为第一舵 机转速系数、x₄为第二舵机转速系数、x₅为第三舵机转速系数；

步骤3、所述输入层向量映射到中间层，所述中间层向量x＝{y₁,x₂,…,y_m}；m为中间层节点个数；

步骤4、得到输出层向量o＝{o₁,o₂,o₃,o₄}；o₁为第一舵机转速调节系数、o₂为第二舵机转速调节系数、o₃为第三舵机转速调节系数、o₄为紧急停机信号；

步骤5、控制第一舵机转速、第二舵机转速和第三舵机转速，使

其中，分别为第i次采样周期输出层向量前三个参数，ω_{a_max}、ω_{b_max}、ω_{c_max}、分别为第一舵机、第二舵机和第三舵机设定的最大转速，ω_a(i+1)、 ω_b(i+1)、ω_c(i+1)、分别为第i+1个采样周期时第一舵机、第二舵机和第三舵机的 设定转速。

优选的是，步骤5之后还包括：根据第i次周期中的运行速度、机器人与 障碍物之间的距离、多个舵机转速，判定第i+1次周期时舵机的运行状态，当 输出信号

时，进行骤停。

优选的是，在所述步骤三中，所述根据所述调节系数对所述机器人是否 发生碰撞进行判断包括如下过程：

计算碰撞系数δ

当碰撞系数δ≥δ′时，判断机器人会发生碰撞；

其中，λ₁+λ₂＝λ₃；

式中，V为运行速度，V₀为校正运行速度，D为机器人与障碍物之间的 距离，D₀为机器人与障碍物之间的校正距离，P₁为第一校正常数，取值范围 为8.13～8.25，P₂为第二校正常数，取值范围为0.13～0.19，o₁为第一舵机转速 调节系数、o₂为第二舵机转速调节系数、o₃为第三舵机转速调节系数，λ₁为 第一舵机转速调节系数权重，λ₂为第二舵机转速调节系数权重，λ₃为第三舵 机转速调节系数权重，δ′为碰撞阈值，取值范围为0.56～0.78。

优选的是，V₀取值为5m/s，D₀取值为5m，P₁取值为8.2，P₂取值为0.15， λ₁取值为0.53，λ₂取值为0.42，λ₃取值为0.95，δ′取值为0.61。

优选的是，所述中间层节点个数m满足：

其中，n为输 入层节点个数，p为输出层节点个数。

优选的是，在所述步骤3中，将所述机器人的运行速度V、所述机器人 与障碍物之间的距离D、第一舵机转速ω_a、第二舵机转速ω_b、第三舵机转速 ω_c进行规格化的公式为：

其中，x_j为输入层向量中的参数，X_j分别为测量参数V、D、ω_a、ω_b、 ω_c，j＝1,2,3,4,5；X_jmax和X_jmin分别为相应测量参数中的最大值和最小值。

优选的是，在所述步骤3中，初始运动状态，第一舵机、第二舵机和第 三舵机的转速满足经验值：

ω_a0＝0.75ω_{a_max}

ω_b0＝0.83ω_{b_max}

ω_c0＝0.88ω_{c_max}

式中，ω_a0、ω_b0、ω_c0分别为第一舵机、第二舵机和第三舵机的初始转速， ω_{a_max}、ω_{b_max}、ω_{c_max}分别为第一舵机、第二舵机和第三舵机的最大转速。

优选的是，在所述步骤3中，初始运动状态，第一舵机、第二舵机和第 三舵机的转速满足经验值：

式中，ω_i0为第i舵机的初始转速，ω_{i_max}为第i舵机的最大转速，V为运行 速度，V₀为校正运行速度，V₀取值为5m/s。

本发明与现有技术相比较所具有的有益效果：本发明通过对机器人的运 行速度监测和与障碍物之间的距离监测等，通过BP神经网络模型对舵机转 速进行调节，再通过计算避障系数进行判断，解决机器人在运行过程中的避 障问题。

具体实施方式

下面对本发明做进一步的详细说明，以令本领域技术人员参照说明书文 字能够据以实施。

本发明提供了一种机器人的运动控制方法，包括如下步骤：

步骤一、分别通过第一测距传感器、第二测距传感器和第三测距传感器 监测机器人运行过程中是否有障碍物，当其中第一测距传感器、第二测距传 感器或者第三测距传感器监测到所述机器人在运行过程中有障碍物时，采集 此时机器人的运行速度、机器人与障碍物之间的距离、第一舵机转速、第二 舵机转速和第三舵机转速；

步骤二、根据机器人此时的运行速度，机器人与障碍物之间的距离，第 一舵机转速、第二舵机转速和第三舵机转速分别对第一舵机转速的调节系数、 第二舵机转速的调节系数和第三舵机转速的调节系数进行控制；

步骤三、分别根据第一舵机转速的调节系数、第二舵机转速的调节系数 和第三舵机转速的调节系数对机器人在运行的过程是否会发生碰撞进行判 断；

步骤四、当判断机器人在运行的过程中会发生碰撞时，对机器人发出停 止信号或者避障转弯信号，当判断机器人在运行的过程中不会发生碰撞时， 对机器人发出继续前进信号。

在本实施例中，第一舵机安装在所述机器人顶部，第二舵机安装在所述 机器人中心位置，第三舵机安装在所述机器人距离底部10cm～25cm处；

第一测距传感器安装在第一舵机的输出轴上，并且由第一舵机控制第一 测距传感器的水平方向上的转角，使第一测距传感器能够沿水平方向监测与 障碍物的距离；

第二测距传感器安装在第二舵机的输出轴上，并且由第二舵机控制第二 测距传感器的水平方向上的转角，使第二测距传感器能够沿水平方向监测与 障碍物的距离；

第三测距传感器安装在第三舵机的输出轴上，并且由第三舵机控制第三 测距传感器的水平方向上的转角，使第三测距传感器能够沿水平方向监测与 障碍物的距离；

其中，第一测距传感器、第二测距传感器和第三测距传感器初始运行过 程中，测距方向依次相差30°～45°；作为一种优选，测距方向依次相差30°。

在步骤二中，本发明基于BP神经网络对所述第一舵机、所述第二舵机 和所述第三舵机的调节系数进行控制，包括如下步骤：

步骤1、建立BP神经网络模型。

本发明采用的BP网络体系结构由三层组成，第一层为输入层，共n个 节点，对应了表示设备工作状态的n个检测信号，这些信号参数由数据预处 理模块给出。第二层为隐层，共m个节点，由网络的训练过程以自适应的方 式确定。第三层为输出层，共p个节点，由系统实际需要输出的响应确定。

该网络的数学模型为：

输入向量：x＝(x₁,x₂,...,x_n)^T

中间层向量：y＝(y₁,y₂,...,y_m)^T

输出向量：O＝(o₁,o₂,...,o_p)^T

本发明中，输入层节点数为n＝5，输出层节点数为p＝4。隐藏层节点数m 由下式估算得出：

输入信号5个参数分别表示为：x₁为运行速度系数、x₂为距离系数、x₃为 第一舵机转速系数、x₄为第二舵机转速系数、x₅为第三舵机转速系数。

由于传感器获取的数据属于不同的物理量，其量纲各不相同。因此，在 数据输入人工神经网络之前，需要将数据规格化为0-1之间的数。

具体而言，对于测量的机器人的运行速度V，进行规格化后，得到运行 速度系数x₁：

其中，V_min和V_max分别为机器人的运行过程中的最小运行速度和最大运行 速度。

同样的，使用测距传感器测量的机器人与障碍物之间的距离D通过下式 进行规格化，得到振动系数x₂：

其中，D_min和D_max分别为测距传感器的最小距离和最大距离。

使用速度传感器测量得到第一舵机的转速ω_a，进行规格化后，得到第一 舵机转速系数x₃：

其中，ω_{a_min}和ω_{a_max}分别为第一舵机转速的最大值和最小值。

使用速度传感器测量得到第二舵机的转速ω_b，进行规格化后，得到第二 舵机转速系数x₄：

其中，ω_{b_min}和ω_{b_max}分别为第二舵机转速的最大值和最小值。

使用速度传感器测量得到第三舵机的转速ω_c，进行规格化后，得到第三 舵机转速系数x₅：

其中，ω_{c_min}和ω_{c_max}分别为第三舵机转速的最大值和最小值。

输出信号的4个参数分别表示为：o₁为第一舵机转速调节系数，o₂为第 二舵机转速调节系数，o₃为第三舵机转速调节系数，o₄为紧急停机信号。

第一舵机转速调节系数o₁表示为下一个采样周期中第一舵机的转速与当 前采样周期中第一舵机的设定最高转速之比，即在第i个采样周期中，采集到 的第一舵机的转速为ω_ai，通过BP神经网络输出第i个采样周期的第一舵机 转速调节系数

后，控制第i+1个采样周期中第一舵机的转速为ω_a(i+1)，使其 满足

第二舵机转速调节系数o₂表示为下一个采样周期中第二舵机的转速与当 前采样周期中第二舵机的设定最高转速之比，即在第i个采样周期中，采集到 的第二舵机的转速为ω_bi，通过BP神经网络输出第i个采样周期的第二舵机 转速调节系数

后，控制第i+1个采样周期中第二舵机的转速为ω_b(i+1)，使其 满足

第三舵机转速调节系数o₃表示为下一个采样周期中第三舵机的转速与当 前采样周期中第三舵机的设定最高转速之比，即在第i个采样周期中，采集到 的第三舵机的转速为ω_ci，通过BP神经网络输出第i个采样周期的第三舵机 转速调节系数

后，控制第i+1个采样周期中第三舵机的转速为ω_c(i+1)，使其 满足

紧急停机信号o₄表示为当前设备的运行状态，其输出值为0或1，当输出 值为0时，表示当前机器人处于非正常状态，此时，需要进行紧急停机；当 输出值为1时，表示当前机器人处于正常状态，可以继续运行。

步骤2、进行BP神经网络的训练。

建立好BP神经网络节点模型后，即可进行BP神经网络的训练。根据产 品的历史经验数据获取训练的样本，并给定输入节点i和隐含层节点j之间的 连接权值w_ij，隐层节点j和输出层节点k之间的连接权值w_jk，隐层节点j的 阈值θ_j，输出层节点k的阈值θ_k、w_ij、w_jk、θ_j、θ_k均为-1到1之间的随机数。

在训练过程中，不断修正w_ij和w_jk的值，直至系统误差小于等于期望误差 时，完成神经网络的训练过程。

如表3所示，给定了一组训练样本以及训练过程中各节点的值。

表3训练过程各节点值

步骤3、采集机器人运行参数输入神经网络得到调控系数及紧急停车信 号。

将训练好的人工神经网络固化在FPGA芯片之中，使硬件电路具备预测 和智能决策功能，从而形成智能硬件。智能硬件加电启动后，第一舵机、第 二舵机和第三舵机均已最大的转速开始运行，即第一舵机初始转速为 ω_a0＝0.75ω_{a_max}，第二舵机初始转速为ω_b0＝0.83ω_{b_max}，第三舵机初始转速为 ω_c0＝0.88ω_{c_max}。

同时采集机器人的初始运行速度V₀、机器人与障碍物之间的初始距离D₀。 通过将上述参数规格化，得到BP神经网络的初始输入向量

通过BP神经网络的运算得到初始输出向量

步骤4、控制第一舵机的转速、第二舵机的转速、第三舵机的转速。

得到初始输出向量

后，即可进行转速的调控，调节第一舵 机的转速、第二舵机的转速和第三舵机的转速，使下一个采样周期第一舵机 的转速、第二舵机的转速和第三舵机的转速分别为：

通过传感器获取第i个采样周期的运行速度V_i、机器人与障碍物之间的距 离D_i、第一舵机转速ω_ai、第二舵机转速ω_bi，第三舵机转速ω_ci，通过进行格式 化得到第i个采样周期的输入向量通过BP神经网络的运 算得到到第i个采样周期的输出向量

然后控制调节第一舵机 的转速、第二舵机的转速和第三舵机的转速，使在第i+1个采样周期时第一 舵机的转速、第二舵机的转速和第三舵机的转速分别为：

步骤5、监测机器人的紧急停车信号以进行紧急停车。

根据

的值判断机器人设置的工作状态，是否处于非正常工作状态，当 机器人处于非正常工作状态时需使设备立即停机，以进行检修，避免设备进 一步的损坏。

通过上述设置，通过传感器实时检测机器人的运行状态，通过采用BP 神经网络算法，对第一舵机、第二舵机、第三舵机进行调控，使机器人达到 最近的运行状态。

在步骤三中，根据第一舵机转速调节系数，第二舵机转速调节系数，第 三舵机转速调节系数对机器人是否发生碰撞进行判断包括如下过程：

计算碰撞系数δ

当碰撞系数δ≥δ′时，判断机器人会发生碰撞；

其中，λ₁+λ₂＝λ₃；

式中，V为运行速度，单位为m/s，V₀为校正运行速度，单位为m/s，D 为机器人与障碍物之间的距离，单位为m，D₀为机器人与障碍物之间的校正 距离，单位为m，P₁为第一校正常数，取值范围为8.13～8.25，P₂为第二校正 常数，取值范围为0.13～0.19，o₁为第一舵机转速调节系数、o₂为第二舵机转 速调节系数、o₃为第三舵机转速调节系数，λ₁为第一舵机转速调节系数权重， λ₂为第二舵机转速调节系数权重，λ₃为第三舵机转速调节系数权重，δ′为碰 撞阈值，取值范围为0.56～0.78。

作为一种优选，在本实施例中，V₀取值为5m/s，D₀取值为5m，P₁取值 为8.2，P₂取值为0.15，λ₁取值为0.53，λ₂取值为0.42，λ₃取值为0.95，δ′取 值为0.61。

在另一种实施例中，初始运动状态，第一舵机、第二舵机和第三舵机的 转速满足经验值：

式中，ω_i0为第i舵机的初始转速，ω_{i_max}为第i舵机的最大转速，V为运行 速度，V₀为校正运行速度，V₀取值为5m/s。

尽管本发明的实施方案已公开如上，但其并不仅仅限于说明书和实施方 式中所列运用，它完全可以被适用于各种适合本发明的领域，对于熟悉本领 域的人员而言，可容易地实现另外的修改，因此在不背离权利要求及等同范 围所限定的一般概念下，本发明并不限于特定的细节。

Claims (10)

1.一种机器人的运动控制方法，其特征在于，包括如下步骤：

步骤一、通过多个测距传感器监测所述机器人运行过程中是否有障碍物，当其中任一所述测距传感器监测到所述机器人在运行过程中有障碍物时，采集所述机器人的运行速度，所述机器人与障碍物之间的距离，多个舵机转速；

步骤二、根据所述运行速度、所述距离和所述舵机转速对所述舵机的调节系数进行控制；

步骤三、根据所述调节系数对所述机器人是否发生碰撞进行判断；

步骤四、当判断所述机器人发生碰撞时，对机器人发出停止信号或者避障转弯信号，当判断所述机器人不发生碰撞时，对机器人发出继续前进信号。

2.根据权利要求1所述的机器人的运动控制方法，其特征在于，在所述步骤一中，所述舵机包括：

第一舵机，其安装在所述机器人顶部；

第二舵机，其安装在所述机器人中心位置；

第三舵机，其安装在所述机器人距离底部10cm～25cm处；以及

所述测距传感器包括：

第一测距传感器，其安装在所述第一舵机的输出轴上，由所述第一舵机控制所述第一测距传感器的水平转角，使所述第一测距传感器能够沿水平方向监测与障碍物的距离；

第二测距传感器，其安装在所述第二舵机的输出轴上，由所述第二舵机控制所述第二测距传感器的水平转角，使所述第二测距传感器能够沿水平方向监测与障碍物的距离；

第三测距传感器，其安装在所述第三舵机的输出轴上，由所述第三舵机控制所述第三测距传感器的水平转角，使所述第三测距传感器能够沿水平方向监测与障碍物的距离；

其中，所述第一测距传感器、所述第二测距传感器和所述第三测距传感器初始运行过程中，测距方向依次相差30°～45°。

3.根据权利要求2所述的机器人的运动控制方法，其特征在于，在所述步骤二中，基于BP神经网络对所述第一舵机、所述第二舵机和所述第三舵机的调节系数进行控制，包括如下步骤：

步骤1、按照采样周期，采集所述机器人的运行速度V，采集所述机器人与障碍物之间的距离D，采集第一舵机转速ω_a、采集第二舵机转速ω_b和采集第三舵机转速ω_c；

步骤2、依次将上述参数进行规格化，确定三层BP神经网络的输入层向量x＝{x₁,x₂,x₃,x₄,x₅}；其中，x₁为运行速度系数、x₂为距离系数、x₃为第一舵机转速系数、x₄为第二舵机转速系数、x₅为第三舵机转速系数；

步骤3、所述输入层向量映射到中间层，所述中间层向量x＝{y₁,x₂,…,y_m}；m为中间层节点个数；

步骤4、得到输出层向量o＝{o₁,o₂,o₃,o₄}；o₁为第一舵机转速调节系数、o₂为第二舵机转速调节系数、o₃为第三舵机转速调节系数、o₄为紧急停机信号；

步骤5、控制第一舵机转速、第二舵机转速和第三舵机转速，使

其中，

分别为第i次采样周期输出层向量前三个参数，ω_{a_max}、ω_{b_max}、ω_{c_max}、分别为第一舵机、第二舵机和第三舵机设定的最大转速，ω_a(i+1)、ω_b(i+1)、ω_c(i+1)、分别为第i+1个采样周期时第一舵机、第二舵机和第三舵机的设定转速。

4.根据权利要求3所述的机器人的运动控制方法，其特征在于，其特征在于，步骤5之后还包括：根据第i次周期中的运行速度、机器人与障碍物之间的距离、多个舵机转速，判定第i+1次周期时舵机的运行状态，当输出信号

时，进行骤停。

5.根据权利要求4所述的机器人的运动控制方法，其特征在于，在所述步骤三中，所述根据所述调节系数对所述机器人是否发生碰撞进行判断包括如下过程：

计算碰撞系数δ

当碰撞系数δ≥δ′时，判断机器人会发生碰撞；

其中，λ₁+λ₂＝λ₃；

式中，V为运行速度，V₀为校正运行速度，D为机器人与障碍物之间的距离，D₀为机器人与障碍物之间的校正距离，P₁为第一校正常数，取值范围为8.13～8.25，P₂为第二校正常数，取值范围为0.13～0.19，o₁为第一舵机转速调节系数、o₂为第二舵机转速调节系数、o₃为第三舵机转速调节系数，λ₁为第一舵机转速调节系数权重，λ₂为第二舵机转速调节系数权重，λ₃为第三舵机转速调节系数权重，δ′为碰撞阈值，取值范围为0.56～0.78。

6.根据权利要求5所述的机器人的运动控制方法，其特征在于，V₀取值为5m/s，D₀取值为5m，P₁取值为8.2，P₂取值为0.15，λ₁取值为0.53，λ₂取值为0.42，λ₃取值为0.95，δ′取值为0.61。

7.根据权利要求4所述的机器人的运动控制方法，其特征在于，所述中间层节点个数m满足：

其中，n为输入层节点个数，p为输出层节点个数。

8.根据权利要求7所述的机器人的运动控制方法，其特征在于，在所述步骤3中，将所述机器人的运行速度V、所述机器人与障碍物之间的距离D、第一舵机转速ω_a、第二舵机转速ω_b、第三舵机转速ω_c进行规格化的公式为：

其中，x_j为输入层向量中的参数，X_j分别为测量参数V、D、ω_a、ω_b、ω_c，j＝1,2,3,4,5；X_jmax和X_jmin分别为相应测量参数中的最大值和最小值。

9.根据权利要求8所述的机器人的运动控制方法，其特征在于，在所述步骤3中，初始运动状态，第一舵机、第二舵机和第三舵机的转速满足经验值：

ω_a0＝0.75ω_{a_max}

ω_b0＝0.83ω_{b_max}

ω_c0＝0.88ω_{c_max}

式中，ω_a0、ω_b0、ω_c0分别为第一舵机、第二舵机和第三舵机的初始转速，ω_{a_max}、ω_{b_max}、ω_{c_max}分别为第一舵机、第二舵机和第三舵机的最大转速。

10.根据权利要求8所述的机器人的运动控制方法，其特征在于，在所述步骤3中，初始运动状态，第一舵机、第二舵机和第三舵机的转速满足经验值：

式中，ω_i0为第i舵机的初始转速，ω_{i_max}为第i舵机的最大转速，V为运行速度，V₀为校正运行速度，V₀取值为5m/s。