CN109940596B - 一种基于方差的机器人位移补偿方法 - Google Patents

Abstract

本发明涉及移动机器人作业精度控制技术领域，提供了一种基于方差的机器人位移补偿方法，包括以下步骤：建立坐标系，根据采样得到的速度数据及旋转数据计算机器人X轴及Y轴上的实时速度；计算机器人相邻两次采样间隔在X轴和Y轴上的相对位移，分别将X轴和Y轴上每次相邻采样间隔的相对位移叠加，得到机器人在X轴及Y轴上的位移；计算机器人在X轴及Y轴方向上的命令速度；通过机器人的实时速度及命令速度计算机器人在X轴及Y轴方向上的速度方差；建立神经网络，通过训练好的神经网络输出X轴及Y轴方向的影响因子；计算机器人在X轴及Y轴方向上的最终位移。通过本发明，有效提高了机器人移动控制精度。

Description

一种基于方差的机器人位移补偿方法

技术领域

本发明属于移动机器人作业精度控制技术领域，具体地说，涉及一种基于方差的机器人位移补偿方法。

背景技术

移动机器人是自动执行工作的机器装置。它既可以接受人类指挥，又可以运行预先编排的程序，也可以根据以人工智能技术制定的原则纲领行动。它的任务是协助或取代人类工作的工作，例如生产业、建筑业，或是危险的工作。

“移动”是机器人的重要属性，移动机器人的发展已有半个世纪的历史。20世纪60年代，斯坦福大学研究所研究出了自主移动机器人Shakey，它可以在复杂的环境下进行对象识别、自主推理、路径规划及控制等功能。70年代，随着计算机技术与传感器技术的发展与应用，移动人机器人的研究出现了新高潮。进入90年代后，随着技术的迅猛发展，移动机器人向实用化、系列化、智能化进军。

轮式移动机器人是移动机器人中应用最多的一种机器人，在相对平坦的地面上，用轮式移动方式是相当优越的。轮式移动机构根据车轮的多少有1轮、2轮、3轮、4轮及多轮机构。1轮及2轮移动机构在实现上的障碍主要是稳定性问题，实际应用的轮式移动机构多采用3轮和4轮。

在实际行走过程中，机器人计算得到的理论位移总会与实际位移有一定误差。机器人的运动是通过改变轮子转速来实现的，但是速度是在随时改变的，通过传感器获得的机器人底盘移动速度会有误差，而地面的摩擦系数等因素也会加大计算的误差，导致机器人无法移动到指定位置。

发明内容

针对现有技术中上述的不足，本发明的目的在于提供一种基于方差的机器人位移补偿方法，通过对计算位移进行补偿来减小误差，接近实际位移。。

为了达到上述目的，本发明采用的解决方案是：

具体的，一种基于方差的机器人位移补偿方法，应用于三轮机器人，所述三轮机器人包括用于采集三轮机器人速度数据的编码器及采集三轮机器人旋转数据的陀螺仪，包括以下步骤：

S1.建立坐标系，根据采样得到的速度数据及旋转数据计算所述三轮机器人的速度v_x及v_y，其中，v_x为三轮机器人在X轴方向上的速度，v_y为三轮机器人在Y轴方向上的速度；

S2.根据v_x、v_y及获取到的所述三轮机器人相对于坐标系的旋转角度θ计算三轮机器人相邻两次采样间隔在X轴上的相对位移Δx及Y轴上的相对位移Δy，分别将X轴和Y轴上每次相邻采样间隔的相对位移叠加，得到三轮机器人在X轴上的位移x及Y轴上的位移y；

S3.通过PID算法分别计算所述三轮机器人在X轴方向上的命令速度v_com_x及在Y轴方向上的命令速度v_com_y；

S4.通过所述三轮机器人的实时速度v_real_x、v_real_y及命令速度v_com_x、v_com_y分别计算三轮机器人在X轴方向上的速度方差

及Y轴方向上的速度方差

其中，v_real_x＝v_x，v_real_y＝v_y；

S5.建立BP神经网络，以v_real_x、v_real_y、

及θ为输入数据，对BP神经网络进行训练直至BP神经网络收敛，通过训练好的BP神经网络输出X轴方向的影响因子K_x及Y轴方向的影响因子K_y；

S6.通过位移计算公式计算所述三轮机器人在X轴方向上的最终计算位移x_total及在Y轴方向上的最终位移y_total，所述位移计算公式如下：

进一步的，所述步骤S1还包括:

通过编码器分别对所述三轮机器人的左轮、右轮及后轮进行速度数据的采样，通过陀螺仪获取三轮机器人绕自身几何中心的旋转速度，建立如下速度模型：

则，由上式可得，

其中，ω₁为三轮机器人左轮的旋转速度，ω₂为三轮机器人右轮的旋转速度，ω₃为三轮机器人后轮的旋转速度，ω为三轮机器人底盘绕自身几何中心的旋转速度,L为三轮机器人的底盘半径，R为左轮、右轮及后轮的半径。

进一步的，分别根据以下公式计算Δx、Δy、x及y：

其中，Δt为相邻两次采样的时间间隔。

进一步的，所述步骤S4包括，速度方差计算公式如下:

其中，n为采样次数。

进一步的，所述BP神经网络包括输入层、隐藏层及输出层，所述输入层及隐藏层的节点数与所述输入数据的维度对应。

进一步的，所述步骤S5还包括：

建立输入层到隐藏层的激活函数g₁(net₁)＝h,net₁＝w^Tp+b₁；

建立隐藏层到输出层的激活函数为

net₂＝v^Th+b₂；

建立计算模型：

其中，g₁为输入层到到隐藏层的激活函数，g₂为隐藏层到输出层的激活函数，w及b₁为输入层到所述隐藏层参数，v及b₂为隐藏层到所述输出层参数，T为矩阵运算符，p为输入层输入数据，

为输出层输出数据预测值。

进一步的，所述BP神经网络训练过程如下：

初始化所述BP神经网络中的权值和偏置项；

激活前向传播，计算损失函数的期望值，所述损失函数为

其中，q为输出层输出数据真实值，n为输出层输出数据维度，m为输出层输出数据组数，λ为参数集合；

执行反向传播，计算输出层误差项：

计算隐藏层误差项：

更新BP神经网络中的权值和偏置项，具体更新过程如下：

更新输出层参数v及b₂：

更新隐藏层参数w及b₁：

重复上述步骤，直至损失函数小于实现给定的阈值或迭代次数用完为止，BP神经网络收敛，得到最佳参数；

其中，η为学习率，k为迭代次数。

本发明的有益效果是：

本发明分别计算机器人在X轴方向及Y轴方向上理论值与实际值的方差，并通过神经网络得到机器人在X轴方向及Y轴方向的影响因子，基于影响因子及方差对机器人位移进行补偿，有效提高了机器人位移的精度。

附图说明

图1为本发明较佳实施例提供的三轮机器人系统结构示意图；

图2为本发明较佳实施例提供的一种基于方差的机器人位移补偿方法流程图；

图3为本发明较佳实施例提供的三轮机器人速度模型示意图；

图4为本发明较佳实施例提供的BP神经网络结构示意图。

具体实施方式

为使本发明实施例的目的、技术方案和优点更加清楚，下面将对本发明实施例中的技术方案进行清楚、完整地描述。实施例中未注明具体条件者，按照常规条件或制造商建议的条件进行。所用试剂或仪器未注明生产厂商者，均为可以通过市售购买获得的常规产品。

下面对本发明实施例提供的一种基于方差的机器人位移补偿方法进行具体说明。

如图1所示，一种基于方差的机器人位移补偿方法，应用于三轮机器人，三轮机器人包括主控板、驱动器、电机、全向轮、编码器及陀螺仪，主控板上设置有处理器，处理器通过内置程序控制驱动器驱动电机转动，从而带动全向轮转动，编码器用于采集三轮机器人左轮、右轮及后轮的转动速度数据并将采样得到的数据反馈给处理器，陀螺仪用于采集包括三轮机器人围绕底盘几何中心旋转的角速度及角度的旋转数据并将采样得到的旋转数据反馈给处理器，如图2所示，本实施例的方法包括以下步骤：

S1.以世界坐标系为本方法的基准坐标系，根据采样得到的速度数据及旋转数据计算三轮机器人的速度v_x及v_y，其中，v_x为三轮机器人在X轴方向上的速度，v_y为三轮机器人在Y轴方向上的速度，v_x及v_y单位均为m/s。

S2.根据v_x、v_y及获取到的三轮机器人相对于坐标系的旋转角度θ计算三轮机器人相邻两次采样间隔在X轴上的相对位移Δx及Y轴上的相对位移Δy，分别将X轴和Y轴上每次相邻采样间隔的相对位移叠加，得到三轮机器人在X轴上的位移x及Y轴上的位移y。

具体过程如下：

通过编码器以一定间隔时间分别对三轮机器人的左轮、右轮及后轮不间断进行转动速度数据的采样，通过陀螺仪获取三轮机器人底盘绕自身几何中心的旋转速度，参见图3，建立三轮机器人全向底盘的速度模型：

则，由上式可得，

其中，ω₁为三轮机器人左轮的转动速度，ω₂为三轮机器人右轮的转动速度，ω₃为三轮机器人后轮的转动速度，ω₁、ω₂及ω₃的单位均为r/min，表示转/每分；ω为三轮机器人底盘绕自身几何中心的旋转速度,单位为rad/s，表示弧度/每秒；L为三轮机器人的底盘半径，R为左轮、右轮及后轮的半径，L及R单位均为m。

计算机器人相邻两次采样在X轴方向和Y轴方向的相对位移Δx及Δy：

将每次相邻采样的相对位移叠加，即可得到机器人在X轴方向及Y轴方向的位移x及y：

其中，Δt为相邻两次采样的时间间隔。

S3.通过PID算法分别计算三轮机器人在X轴方向上的命令速度v_com_x及在Y轴方向上的命令速度v_com_y，PID算法公式如下：

Δu(k)＝u(k)-u(k-1)＝K_p(e(k)-e(k-1))+K_ie(k)+K_d(e(k)-2e(k-1)+e(k-2))，

u(k)＝Δu(k)+u(k-1)；

其中，K_p为比例系数，K_i为积分系数，K_d为微分系数，e(k)为偏差，k为时刻，u为速度，则通过上述PID算法公式即可得到三轮机器人在X轴方向上的命令速度v_com_x及在Y轴方向上的命令速度v_com_y。

S4.通过三轮机器人的实时速度v_real_x、v_real_y及命令速度v_com_x、v_com_y分别计算三轮机器人在X轴方向上的速度方差

及Y轴方向上的速度方差

其中，v_real_x＝v_x，v_real_y＝v_y。

由于采用PID算法控制底盘速度，即使命令速度不变，实际速度也会以命令速度为中心做微小震荡，同时，由编码器测得脉冲计算得到的底盘实时速度是短时间内的平均速度，上述因素都会加大底盘位移计算的误差，本发明通过分别计算机器人在X轴方向和Y轴方向上的速度方差来对机器人位移进行补偿。

速度方差计算公式如下:

其中，n为采样次数。

S5.建立BP神经网络，参照图4，BP神经网络包括输入层、隐藏层及输出层，输入层及隐藏层的节点数与输入数据的维度对应，本实施例中，BP神经网络的输入数据为5维，输出数据为2维；以v_real_x、v_real_y、

及θ为输入数据，对BP神经网络进行训练直至BP神经网络收敛，通过训练好的BP神经网络输出X轴方向的影响因子K_x及Y轴方向的影响因子K_y，具体过程如下：

S51.建立如下BP神经网络模型：

建立输入层到隐藏层的激活函数g₁(net₁)＝h,net₁＝w^Tp+b₁；

建立隐藏层到输出层的激活函数为

net₂＝v^Th+b₂；

建立计算模型：

损失函数为：

其中，g₁为输入层到到隐藏层的激活函数，g₂为隐藏层到输出层的激活函数，w及b₁为输入层到隐藏层参数，v及b₂为隐藏层到输出层参数，其中，w及v为权重，b₁及b₂为偏置项，T为矩阵运算符，p为输入层输入数据，

为输出层输出数据预测值，q为输出层输出数据真实值，n为输出层输出数据维度，m为输出层输出数据组数，λ为参数集合，参数包括权值和偏置项。

本实施例中各参数集合如下：

S52.以v_real_x、v_real_y、

及θ为输入，对BP神经网络进行训练，具体过程如下：

初始化BP神经网络中的权值和偏置项，分别记为w⁽⁰⁾,

v⁽⁰⁾,

激活前向传播，计算损失函数的期望值，

表示对总的误差值取平均，输出数据多少维，则误差值求平均就除以多少，本实施例中，模型输出数据为2维，故n为2，输出数据为多组，输出数据为n维m组，则真实值与输出值表示为q_n×m,

上述损失函数可表示为

计算模型预测值与真实值的误差，执行反向传播，将该误差反向传播到隐藏层，计算各层误差，过程如下：

计算输出层误差项，即计算损失函数关于输出层的偏导数，根据链式法则有：

计算隐藏层误差项，即计算损失函数关于隐藏层的偏导数，根据链式法则有：

更新BP神经网络中的权值和偏置项，具体更新过程如下：

更新输出层参数v及b₂：

更新隐藏层参数w及b₁：

其中，η为学习率，k为迭代次数，k＝1,2,...,n，表示更新次数或迭代次数，k＝1表示第一次更新，以此类推。

S53.重复上述步骤，直至损失函数小于给定的阈值或迭代次数用完为止，BP神经网络收敛，得到最佳权值和偏置项，在当前输出的权值和偏置项下，模型具有最小误差。

S6.通过位移计算公式计算三轮机器人在X轴方向上的最终计算位移x_total及在Y轴方向上的最终位移y_total，位移计算公式如下：

执行反向传播的过程中，由于机器人在X轴方向及Y轴方向上的速度方差

和

及机器人在X轴方向及Y轴方向上的位移x和y均可以通过计算得到，机器人在X轴方向及Y轴方向上的真实位移x_total及y_total也能直接获取，则真实值q可以通过位移计算公式反推得到。

经测试，当控制机器人以0.1m/s的速度运动时，随着运动时间的增长，机器人的计算值与理论值之间存在误差，且误差不断增大，补偿前，机器人运动200s后，计算值与理论值之间的误差接近50cm，而经补偿后的计算值与理论值之间的误差约为5cm，补偿计算后误差明显减小。

当控制机器人以0.2m/s的速度运动时，随着运动时间的增长，机器人位移的理论值与计算值之间存在误差，且误差不断增大，且误差比速度为0.1m/s时的误差大得多。补偿前，机器人运动200s后，计算值与理论值之间的误差接近78cm，而经补偿后的计算值与理论值之间的误差约为7cm，补偿计算后误差明显减小。

综上所述，本发明通过分别计算机器人在X轴方向及Y轴方向上理论值与实际值的方差，并通过建立BP神经网络模型，训练模型调整权值和偏置项，得到使机器人在X轴方向及Y轴方向误差最小的影响因子，基于影响因子及X轴、Y轴的速度方差对机器人的位移进行补偿，有效的提高了机器人位移控制精度，解决了机器人在移动过程中因速度控制算法、反馈数据采集及机械摩擦等因素造成的实际位移与理论值偏差越来越大的问题。

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

以上仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (7)

1.一种基于方差的机器人位移补偿方法，应用于三轮机器人，所述三轮机器人包括用于采集三轮机器人速度数据的编码器及采集三轮机器人旋转数据的陀螺仪，其特征在于，包括以下步骤：

S1.建立坐标系，根据采样得到的速度数据及旋转数据计算所述三轮机器人的速度v_x及v_y，其中，v_x为三轮机器人在X轴方向上的速度，v_y为三轮机器人在Y轴方向上的速度；

S2.根据v_x、v_y及获取到的所述三轮机器人相对于坐标系的旋转角度θ计算三轮机器人相邻两次采样间隔在X轴上的相对位移Δx及Y轴上的相对位移Δy，分别将X轴和Y轴上每次相邻采样间隔的相对位移叠加，得到三轮机器人在X轴上的位移x及Y轴上的位移y；

S3.通过PID算法分别计算所述三轮机器人在X轴方向上的命令速度v_com_x及在Y轴方向上的命令速度v_com_y；

S4.通过所述三轮机器人的实时速度v_real_x、v_real_y及命令速度v_com_x、v_com_y分别计算三轮机器人在X轴方向上的速度方差

及Y轴方向上的速度方差

其中，v_real_x＝v_x，v_real_y＝v_y；

S5.建立BP神经网络，以v_real_x、v_real_y、

及θ为输入数据，对BP神经网络进行训练直至BP神经网络收敛，通过训练好的BP神经网络输出X轴方向的影响因子K_x及Y轴方向的影响因子K_y；

S6.通过位移计算公式计算所述三轮机器人在X轴方向上的最终计算位移x_total及在Y轴方向上的最终位移y_total，所述位移计算公式如下：

2.根据权利要求1所述的一种基于方差的机器人位移补偿方法，其特征在于，所述步骤S1还包括:

通过编码器分别对所述三轮机器人的左轮、右轮及后轮进行速度数据的采样，通过陀螺仪获取三轮机器人绕自身几何中心的旋转速度，建立如下速度模型：

则，由上式可得，

其中，ω₁为三轮机器人左轮的转动速度，ω₂为三轮机器人右轮的转动速度，ω₃为三轮机器人后轮的转动速度，ω为三轮机器人底盘绕自身几何中心的旋转速度,L为三轮机器人的底盘半径，R为左轮、右轮及后轮的半径。

3.根据权利要求1所述的一种基于方差的机器人位移补偿方法，其特征在于，分别根据以下公式计算Δx、Δy、x及y：

其中，Δt为相邻两次采样的时间间隔。

4.根据权利要求1所述的一种基于方差的机器人位移补偿方法，其特征在于，所述步骤S4包括，速度方差计算公式如下:

其中，n为采样次数。

5.根据权利要求1所述的一种基于方差的机器人位移补偿方法，其特征在于，所述BP神经网络包括输入层、隐藏层及输出层，所述输入层及隐藏层的节点数与所述输入数据的维度对应。

6.根据权利要求5所述的一种基于方差的机器人位移补偿方法，其特征在于，所述步骤S5还包括：

建立输入层到隐藏层的激活函数g₁(net₁)＝h,net₁＝w^Tp+b₁；

建立隐藏层到输出层的激活函数为

建立计算模型：

其中，g₁为输入层到隐藏层的激活函数，g₂为隐藏层到输出层的激活函数，w及b₁为输入层到所述隐藏层参数，v及b₂为隐藏层到所述输出层参数，T为矩阵运算符，p为输入层输入数据，

为输出层输出数据预测值。

7.根据权利要求6所述的一种基于方差的机器人位移补偿方法，其特征在于，所述BP神经网络训练过程如下：

初始化所述BP神经网络中的权值和偏置项；

激活前向传播，计算损失函数的期望值，所述损失函数为

其中，q为输出层输出数据真实值，n为输出层输出数据维度，m为输出层输出数据组数，λ为参数集合；

执行反向传播，计算输出层误差项：

计算隐藏层误差项：

更新BP神经网络中的权值和偏置项，具体更新过程如下：

更新输出层参数v及b₂：

更新隐藏层参数w及b₁：

重复上述步骤，直至损失函数小于实现给定的阈值或迭代次数用完为止，BP神经网络收敛，得到最佳参数；

其中，η为学习率，k为迭代次数。

Images