CN110794764B - 一种机器人的运动控制方法 - 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。

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

i＝a,b,c；

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

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

Claims (8)

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。

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所述的机器人的运动控制方法，其特征在于，所述中间层节点个数m满足：

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

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

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

7.根据权利要求6所述的机器人的运动控制方法，其特征在于，在所述步骤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}分别为第一舵机、第二舵机和第三舵机的最大转速。

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

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