CN110543919A - 一种机器人定位控制方法、终端设备及存储介质 - Google Patents

Abstract

本发明涉及一种机器人定位控制方法、终端设备及存储介质，该方法中包括：S1：采集不同时刻机器人的图像特征，构建小波Elman网络模型，将采集的图像特征作为模型的输入，系统定位的状态向量作为模型的输出，对模型的参数进行训练，以使模型输出的状态向量跟踪通过卡尔曼滤波算法预测的状态向量，将训练后的模型作为最终模型；S2：将机器人的图像特征I(t)输入最终模型得到最终状态向量X₁(t)，将最终状态向量X₁(t)转换为微分矩阵G(t)；S3：根据微分矩阵G(t)计算机器人的控制量U^R(t)；S4：根据机器人的控制量U^R(t)对机器人的运动进行控制。本发明实现了机器人单目视觉大空间定位控制，既能保证机器人系统鲁棒稳定，又使得伺服系统具有一定的环境自适应性。

Description

一种机器人定位控制方法、终端设备及存储介质

技术领域

本发明涉及机器人技术领域，尤其涉及一种机器人定位控制方法、终端设备及存储介质。

背景技术

机器人技术日新月异，应用领域不断推广，特别是智能服务型机器臂工作环境特殊，此类机器人要求执行多样任务，应对多变环境，以致自身定位控制技术面临挑战。视觉传感利用视觉反馈信息调整机器人姿态，由此构成视觉闭环反馈控制技术，有利于提高机器人任务操作的灵活性，与环境自适应性，在机器人操作研究中具有重要地位。

机器人视觉反馈控制技术广泛分为基于位置的视觉伺服(PBVS)和基于图像的视觉伺服(IBVS)。PBVS方法其计算基础是视觉测量技术，视觉系统通过图像特征计算目标物3D笛卡尔坐标位置，并根据已知的目标几何模型，与机器人系统标定参数估计目标姿态，伺服控制器再根据机器人与目标之间的相对位姿进行轨迹规划并实施任务操作。与PBVS过分依赖于目标3D重建技术不同，IBVS视觉伺服直接以2D图像特征为反馈信息，将当前图像特征与期望特征之间的图像误差映射到机器人运动空间中，由图像误差计算机器人控制量并实施运动控制。IBVS省略了目标3D重建而得到广泛关注。然而，IBVS视觉伺服控制技术过于依赖映射微分矩阵，求解非线性微分矩阵是实现IBVS方法的前提条件，而现有大部分求解方法受限于机器人严格标定条件。因此在非结构非标定环境中，建立新型无模型视觉伺服控制方法意义显著。

发明内容

为了解决上述问题，本发明提出了一种机器人定位控制方法、终端设备及存储介质。

具体方案如下：

一种机器人定位控制方法，包括以下步骤：

S1：采集不同时刻机器人的图像特征，构建小波Elman网络模型，将采集的图像特征作为模型的输入，系统定位的状态向量作为模型的输出，对模型的参数进行训练，以使模型输出的状态向量跟踪通过卡尔曼滤波算法预测的状态向量，将训练后的模型作为最终模型；

S2：将机器人的图像特征I(t)输入最终模型得到最终状态向量X₁(t)，将最终状态向量X₁(t)转换为微分矩阵G(t)，其中，x_n*m(t)＝g_nm(t)，x_n*m表示向量X₁(t)的第n×m个元素，g_nm表示矩阵G(t)的第n行中第m列的元素；

S3：根据微分矩阵G(t)计算机器人的控制量U^R(t)：

U^R(t)＝λΔtG(t)^T(G(t)G(t)^T)^-1I_e(t)+U^R(t-1)

I_e(t)＝I(t)-I^*

其中，上标T表示矩阵的转置，λ为控制系数，Δt表示时刻t和时刻t-1的时间差，I_e(t)为t时刻的图像特征误差，I^*为机器人的期望图像特征；

S4：根据机器人的控制量U^R(t)对机器人的运动进行控制。

进一步的，步骤S1具体包括以下步骤：

S11：将t时刻的图像特征I(t)作为小波Elman网络模型的输入，模型输出机器人系统定位的次优状态向量X'(t)；

S12：设定系统的状态转移方程为：

其中，X(t)和X(t-1)分别为t时刻和t-1时刻的状态向量，Ψ(t/t-1)为t-1至t时刻的状态转移矩阵，为系统噪声；

令X(t-1)＝X'(t)，根据系统的状态转移方程计算获得系统的状态向量X(t)；

S13：根据t时刻系统的状态向量X(t)和系统的状态观测方程，计算t时刻的状态观测值Z(t)；

状态观测方程为：

Z(t)＝Φ(t)X(t)+ψ(t)

其中，Φ(t)为观测矩阵，ψ(t)为观测噪声；

S14：采用卡尔曼滤波算法，并根据系统的状态转移方程和状态观测方程，对系统定位状态进行预测，得到t时刻机器人系统定位的最优状态估计向量

S15：以图像特征I(t)为输入量，以步骤S14得到的最优状态估计向量为参考值，采用梯度下降法对t时刻小波Elman网络模型的权值进行调整，以实现小波Elman网络模型的输出X'(t)跟踪卡尔曼滤波算法的输出

进一步的，小波Elman网络模型的构建方法为：

步骤1：输入层神经元将输入的图像特征传递至隐含层，输入层神经元的传输函数为线性函数，输入层每个神经元节点i的输入和输出分别为：

其中，和分别为输入层第i个节点的输入和输出值，上标表示层，下标i＝1,2,...,m⁽¹⁾表示神经元的序号，m⁽¹⁾表示输入层神经元的总数；

步骤2：关联层对隐含层输出和自身层输出进行反馈传输，关联层每个神经元节点j的输入和输出分别为：

其中，和分别为关联层第j个节点的输入和输出值，为隐含层的输出值，下标j＝1,2,...,m⁽²⁾表示神经元的序号，m⁽²⁾表示关联层和隐含层神经元总数，0≤γ≤1为关联层反馈增益；

步骤3：反馈层神经元对输出层实现非线性反馈传递，反馈层每个神经元节点k的输入和输出分别为：

其中，和分别为反馈层第k个节点的输入和输出值，为输出层的输出值，上标表示层，下标k＝1,2,...,m⁽³⁾表示神经元的序号，m⁽³⁾表示反馈层神经元总数，为神经元非线性激励函数；

步骤4：隐含层神经元激励函数采用小波函数，隐含层每个神经元节点l的输入和输出表示为：

h(x)＝-xexp[-x²/2]

其中，和分别为隐含层第l个节点的输入和输出值，a_l(t)为小波伸缩系数，b_l(t)为小波平移系数，上标表示层，下标l＝1,2,...,m⁽⁴⁾表示神经元的序号，m⁽⁴⁾表示神经元总数；

步骤5：输出层每个节点c的输入和输出表示为：

其中，和分别为输出层第c个节点的输入和输出值，上标表示层，下标c＝1,2,...,m⁽⁵⁾表示神经元的序号，m⁽⁵⁾表示神经元总数，X′(t)为机器人系统定位的次优状态向量。

进一步的，步骤S14的系统定位状态预测过程包括：

步骤1：初始化设置：

其中，G(0)和X(0)分别表示初始状态的微分矩阵和状态向量，Q和R均为噪声方差矩阵；

步骤2：计算中间状态向量

步骤3：预测误差协方差P(t/t-1)：P(t/t-1)＝Ψ(t/t-1)P(t-1)Ψ(t/t-1)^T+Q；

步骤4：计算卡尔曼滤波增益K(t)：K(t)＝P(t/t-1)Φ(t)^T(Φ(t)P(t/t-1)Φ(t)^T+R)^-1；

步骤5：计算t时刻机器人系统定位最优状态估计向量

进一步的，步骤S15中权值w_l(t)更新规则为：

其中，η表示学习率。

一种机器人定位控制终端设备，包括处理器、存储器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现本发明实施例上述的方法的步骤。

一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现本发明实施例上述的方法的步骤。

本发明采用如上技术方案，基于状态空间学习技术，建立图像空间与机器人运动空间映射模型，并且针对IBVS视觉伺服技术的局限，在线学习求解机器人视觉-运动空间的非线性微分矩阵，实现了机器人单目视觉大空间定位控制，既能保证机器人系统鲁棒稳定，又使得伺服系统具有一定的环境自适应性，适用于机器人本体无模型系统，无需要标定相机内部参数，免去了复杂的标定过程。

附图说明

图1所示为本发明实施例一中整体的控制流程图。

图2所示为该实施例中小波Elman网络模型示意图。

图3所示为该实施例中PBVS方法机器人运动轨迹图。

图4所示为该实施例中PBVS方法机器人定位图像误差图。

图5所示为该实施例中PBVS方法机器人图像平面特征点运动轨迹图。

图6所示为该实施例中IBVS方法机器人运动轨迹图。

图7所示为该实施例中IBVS方法机器人定位图像误差图。

图8所示为该实施例中IBVS方法机器人图像平面特征点运动轨迹图。

图9所示为该实施例中方法机器人运动轨迹图。

图10所示为该实施例中方法机器人定位图像误差图。

图11所示为该实施例中方法机器人图像平面特征点运动轨迹图。

图12所示为该实施例中传统卡尔曼方法的机器人运动轨迹图。

图13所示为该实施例中传统卡尔曼方法的机器人图像平面特征点运动轨迹图。

图14所示为该实施例中传统卡尔曼方法的机器人定位图像误差图。

图15所示为该实施例中方法的机器人运动轨迹图。

图16所示为该实施例中方法的机器人图像平面特征点运动轨迹图。

图17所示为该实施例中方法的机器人定位图像误差图。

具体实施方式

为进一步说明各实施例，本发明提供有附图。这些附图为本发明揭露内容的一部分，其主要用以说明实施例，并可配合说明书的相关描述来解释实施例的运作原理。配合参考这些内容，本领域普通技术人员应能理解其他可能的实施方式以及本发明的优点。

现结合附图和具体实施方式对本发明进一步说明。

实施例一：

参考图1所示，本发明实施例提供了一种机器人定位控制方法，包括以下步骤：

S1：采集不同时刻机器人的图像特征，构建小波Elman网络模型，将采集的图像特征作为模型的输入，系统定位的状态向量作为模型的输出，对模型的参数进行训练，以使模型输出的状态向量跟踪通过卡尔曼滤波算法预测的状态向量，将训练后的模型作为最终模型。

所述图像特征可以通过相机或摄像机进行采集，该实施例中以六自由度单目工业机器人为实验平台，摄像机固定安装在机器人手臂末端，构成“眼在手”(Eye-In-Hand)机器人定位系统。

该实施例中采用反馈型小波Elman神经网络对机器人视觉-运动空间的非线性映射关系进行全局学习，以实现机器人在大范围内定位到期望位姿的邻域范围内。如图2所示，该实施例采用五层小波Elman神经网络拓扑结构，网络模型的构建过程如下：

步骤1：输入层神经元将输入的图像特征传递至隐含层，输入层神经元的传输函数为线性函数，输入层每个神经元节点i的输入和输出分别为：

其中，和分别为输入层第i个节点的输入和输出值，上标表示层，下标i＝1,2,...,m⁽¹⁾表示神经元的序号，m⁽¹⁾表示输入层神经元的总数；

步骤2：关联层对隐含层输出和自身层输出进行反馈传输，关联层每个神经元节点j的输入和输出分别为：

其中，和分别为关联层第j个节点的输入和输出值，为隐含层的输出值，下标j＝1,2,...,m⁽²⁾表示神经元的序号，m⁽²⁾表示关联层和隐含层神经元总数，0≤γ≤1为关联层反馈增益；

步骤3：反馈层神经元对输出层实现非线性反馈传递，反馈层每个神经元节点k的输入和输出分别为：

其中，和分别为反馈层第k个节点的输入和输出值，为输出层的输出值，上标表示层，下标k＝1,2,...,m⁽³⁾表示神经元的序号，m⁽³⁾表示反馈层神经元总数，为神经元非线性激励函数；

步骤4：隐含层神经元激励函数采用小波函数，隐含层每个神经元节点l的输入和输出表示为：

h(x)＝-xexp[-x²/2]

其中，和分别为隐含层第l个节点的输入和输出值，a_l(t)为小波伸缩系数，b_l(t)为小波平移系数，上标表示层，下标l＝1,2,...,m⁽⁴⁾表示神经元的序号，m⁽⁴⁾表示神经元总数；

步骤5：输出层每个节点c的输入和输出表示为：

其中，和分别为输出层第c个节点的输入和输出值，上标表示层，下标c＝1,2,...,m⁽⁵⁾表示神经元的序号，m⁽⁵⁾表示神经元总数，X′(t)为机器人系统定位的次优状态向量。

在实际的机器人操作中，由于机器人全局工作空间学习样本稀疏有限，机器人定位精度不高，为了解决该问题，该实施例中通过构建卡尔曼(Kalman)滤波联合反馈型小波Elman神经网络学习算法，对小波Elman网络模型的输出状态实施卡尔曼滤波修正，具体包括以下步骤：

S11：将t时刻的图像特征I(t)作为小波Elman网络模型的输入，模型输出机器人系统定位的次优状态向量X'(t)。

S12：设定系统的状态转移方程为：

其中，X(t)和X(t-1)分别为t时刻和t-1时刻的状态向量，Ψ(t/t-1)为t-1至t时刻的状态转移矩阵，为系统噪声。

令X(t-1)＝X'(t)，根据系统的状态转移方程计算获得系统的状态向量X(t)。

S13：根据t时刻系统的状态向量X(t)和系统的状态观测方程，计算t时刻的状态观测值Z(t)。

状态观测方程为：

Z(t)＝Φ(t)X(t)+ψ(t)

其中，Φ(t)为观测矩阵，ψ(t)为观测噪声。

该实施例中，观测矩阵Φ(t)为：

其中，表示机器人控制量U^R(t)的导数。

S14：采用卡尔曼滤波算法，并根据系统的状态转移方程和状态观测方程，对系统定位状态进行预测，得到t时刻机器人系统定位的最优状态估计向量

步骤S14具体包括以下步骤：

步骤1：初始化设置：

其中，G(0)和X(0)分别表示初始状态的微分矩阵和状态向量，0表示t＝0，Q和R均为噪声方差矩阵；

步骤2：计算中间状态向量

步骤3：预测误差协方差P(t/t-1)：P(t/t-1)＝Ψ(t/t-1)P(t-1)Ψ(t/t-1)^T+Q；

步骤4：计算卡尔曼滤波增益K(t)：K(t)＝P(t/t-1)Φ(t)^T(Φ(t)P(t/t-1)Φ(t)^T+R)^-1；

步骤5：计算t时刻机器人系统定位最优状态估计向量

S15：以图像特征I(t)为输入量，以步骤S14得到的最优状态估计向量为参考值，采用梯度下降法对t时刻小波Elman网络模型的权值进行调整，以实现小波Elman网络模型的输出X'(t)跟踪卡尔曼滤波算法的输出

权值w_l(t)的更新规则为：

其中，η表示学习率。

通过更新权值w_l(t)以使代价函数Υ(t)的值最小：

S2：将机器人的图像特征I(t)输入最终模型得到最终状态向量X₁(t)，将最终状态向量X₁(t)转换为微分矩阵G(t)，其中，x_n*m(t)＝g_nm(t)，x_n*m表示向量X₁(t)的第n×m个元素，g_nm表示矩阵G(t)的第n行中第m列的元素。

S3：根据微分矩阵G(t)计算机器人的控制量U^R(t)。

通过图像特征I(t)与机器人的控制量U^R(t)之间的微分方程，构建机器人的视觉-运动空间映射模型为：

其中，为非线性时变微分矩阵。

由于在非结构环境中其微分矩阵未知，因此，该实施例中把非线性时变微分矩阵转化为系统在线状态估计问题，为此定义不显式微分矩阵G(t)：

由于在从图像中采集到的图像特征对应的位姿与期望位姿有一定的误差，因此，设定图像特征误差I_e(t)为：

I_e(t)＝I(t)-I^*

其中，为t时刻机器人的图像特征，为机器人的期望图像特征。

对上式进行求导，由于其中的期望图像特征I^*为定值，因此可得：

假定存在非零常数λ，使得以下等式成立：

则有：

因此，机器人的视觉-运动空间映射模型可变换为：

-λI_e(t)Δt＝G(t)ΔU^R(t)

其中，Δt表示时刻t和时刻t-1的时间差，ΔU^R(t)表示时刻t和时刻t-1控制量U^R(t)的差值。

上式变形可得：

ΔU^R(t)＝-λΔtG(t)^T(G(t)G(t)^T)^-1I_e(t)

设定上式中的非零常数λ表示控制系数，则t时刻机器人的控制量U^R(t)为：

U^R(t)＝ΔU^R(t)+U^R(t-1)

即：

U^R(t)＝λΔtG(t)^T(G(t)G(t)^T)^-1I_e(t)+U^R(t-1)

S4：根据机器人的控制量U^R(t)对机器人的运动进行控制。

机器人控制量U^R(t)＝(v^R(t),w^R(t))^T，其中，v^R(t)＝(v_x(t),v_y(t),v_z(t))，w^R(t)＝(w_x(t),w_y(t),w_z(t))分别为机器人末端在基坐标系的x、y、z方向上的线速度和角速度。

实验效果

该实施例中设定相关参数如下，小波Elman网络模型的输入层神经元个数m⁽¹⁾＝8，隐含层和关联层神经元个数m⁽²⁾＝m⁽⁴⁾＝87，输出层和输出反馈层神经元个数m⁽³⁾＝m⁽⁵⁾＝48，小波伸缩系数a_l(t)＝0.1，小波平移系数b_l(t)＝0.4，网络学习率η＝0.15，控制系数λ＝0.2。

以经典PBVS和IBVS方法为比较对象，实验中主要验证本实施例方法的两方面的性能：一方面为机器人工作空间大范围定位性能，另一方面为机器人定位鲁棒稳定性能。为了体现机器人大范围定位，该实施例中设定机器人的初始位姿远离期望位姿，即分别设定在机器人工作空间的最远边缘和最近边缘处。

PBVS方法的实验结果如图3、图4和图5所示。由图3可知，在大空间范围内PBVS方法机器人末端从初始位姿定位到期望位姿，机器人末端以直线运动轨迹从初始位姿定位到期望位姿，这是因为PBVS控制器在笛卡尔空间内以机器人位姿为控制对象，直接控制机器人运动轨迹，能够确保机器人运动轨迹稳定在工作范围内。图4为机器人定位图像误差，可知PBVS伺服方法图像误差收敛趋向于零，表明机器人任务完成。然而从图5可以看出，特征点在图像空间内运动轨迹绕行，特征点已经超出相机视场范围，导致特征容易消失在机器人单目可视范围内，这是因为PBVS控制器难易兼顾图像特征点的运动轨迹控制。

IBVS方法实验结果如图6、图7和图8所示。由图8可看出，IBVS方法图像特征以直线运动轨迹从初始位置收敛到期望位置，特征点始终保持在相机视场范围内，这是因为IBVS控制器在图像空间内以特征点位置为控制对象，直接控制特征点运动轨迹，能够确保特征运动轨迹稳定在视觉范围内。并且由图7可知，IBVS方法图像误差收敛趋向于零，表明机器人定位完成。但是图6表明，机器人末端笛卡尔空间运动轨迹不稳定，机器人运动产生较大震动，容易超出机器人工作范围内，这是因为IBVS方法难易兼顾到机器人的运动轨迹控制。

本发明方法实验结果如图9、图10和图11所示。由图9可看出，本实施例方法机器人定位以接近直线运动从始位姿定位到期望位姿，机器人运动轨迹稳定无震荡，图10为机器人定位误差，图像误差收敛趋向于零，机器人定位成功。同时由图11可知，图像特征轨迹同样以近直线方式从初始位置收敛到期望位置，并且特征点保持在相机视场范围内。

通过以上机器人大范围定位实验比较可知，PBVS方法图像特征轨迹容易偏离相机视场范围，IBVS方法机器人运动轨迹震荡不稳定，容易超出机器人工作范围，而本实施例方法机器人运动轨迹平稳无震动，图像特征轨迹保持在相机视场范围内，综合性能最好。

另外，本实施例与传统卡尔曼滤波方法比较，进一步说明本发明小波Elman网络联合卡尔曼方法的鲁棒稳定性。在机器人实际环境操作中，系统噪声和传感器观测噪声统计信息的不确定性，将影响机器人定位性能，严重时可导致系统不稳定定位失败，因此，本实施例加入相关白噪声序列测试系统的鲁棒性能，系统噪声均值为0，方差为Q＝2.1，观测噪声均值为0，方差分别为R＝1.4。

传统卡尔曼方法机器人定位结果如图12和图13所示。由图12可知，在外界噪声干扰影响下机器人末端轨迹大幅度绕行运动，机器人运动先远离期望位姿而后逐渐接近期望位姿，图13可以看出机器人这种绕行运动进而导致图像特征轨迹偏离相机视场范围。

面对相同外界干扰机器人定位任务，该实施例中采用卡尔曼联合小波Elman学习的方法的实验结果如图15和图16所示。由图15可看出，机器人运动直接定位到期望位姿，轨迹无绕行并且机器人运动稳定，同时由图16可知，图像特征轨迹直行平滑，特征点保持在机器人视场范围内。

另外，由图14和17机器人定位图像误差图的对比可以看出，本实施例采用的卡尔曼联合小波Elman网络学习方法比传统卡尔曼方法收敛快，图像误差小，以上结果主要原因是小波Elman网络预先得到机器人全局定位的次优状态，在这基础上卡尔曼实施状态最优滤波，并实时调整网络权重，这种分步联合工作方式有利于提高机器人执行效率和提高定位精度，并且保证机器人全局空间的稳定性。

本发明实施例一首先获取图像特征，小波Elman网络模型对图像特征进行学习，输出机器人定位系统的次优状态；在此基础上利用卡尔曼滤波算法对模型输出的次优状态进行滤波修正，并恢复得到机器人视觉-运动空间微分矩阵的精确估计值，为此提高机器人定位精度；其次小波Elman网络以卡尔曼状态估计值为参考值在线更新权值，使网络输出状态逼近卡尔曼状态估计值，从而抑制系统状态跳变，确保系统稳定；最后根据更新的权值输出状态向量得到对应的微分矩阵，通过微分矩阵得到机器人的控制量，进而控制机器人末端从当前位姿定位到期望位姿。

实施例二：

本发明还提供一种机器人定位控制终端设备，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现本发明实施例一的上述方法实施例中的步骤。

进一步地，作为一个可执行方案，所述机器人定位控制终端设备可以是桌上型计算机、笔记本、掌上电脑及云端服务器等计算设备。所述机器人定位控制终端设备可包括，但不仅限于，处理器、存储器。本领域技术人员可以理解，上述机器人定位控制终端设备的组成结构仅仅是机器人定位控制终端设备的示例，并不构成对机器人定位控制终端设备的限定，可以包括比上述更多或更少的部件，或者组合某些部件，或者不同的部件，例如所述机器人定位控制终端设备还可以包括输入输出设备、网络接入设备、总线等，本发明实施例对此不做限定。

进一步地，作为一个可执行方案，所称处理器可以是中央处理单元(CentralProcessing Unit，CPU)，还可以是其他通用处理器、数字信号处理器(Digital SignalProcessor，DSP)、专用集成电路(Application Specific Integrated Circuit，ASIC)、现场可编程门阵列(Field-Programmable Gate Array，FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等，所述处理器是所述机器人定位控制终端设备的控制中心，利用各种接口和线路连接整个机器人定位控制终端设备的各个部分。

所述存储器可用于存储所述计算机程序和/或模块，所述处理器通过运行或执行存储在所述存储器内的计算机程序和/或模块，以及调用存储在存储器内的数据，实现所述机器人定位控制终端设备的各种功能。所述存储器可主要包括存储程序区和存储数据区，其中，存储程序区可存储操作系统、至少一个功能所需的应用程序；存储数据区可存储根据手机的使用所创建的数据等。此外，存储器可以包括高速随机存取存储器，还可以包括非易失性存储器，例如硬盘、内存、插接式硬盘，智能存储卡(Smart Media Card,SMC)，安全数字(Secure Digital,SD)卡，闪存卡(Flash Card)、至少一个磁盘存储器件、闪存器件、或其他易失性固态存储器件。

本发明还提供一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序被处理器执行时实现本发明实施例上述方法的步骤。

所述机器人定位控制终端设备集成的模块/单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明实现上述实施例方法中的全部或部分流程，也可以通过计算机程序来指令相关的硬件来完成，所述的计算机程序可存储于一计算机可读存储介质中，该计算机程序在被处理器执行时，可实现上述各个方法实施例的步骤。其中，所述计算机程序包括计算机程序代码，所述计算机程序代码可以为源代码形式、对象代码形式、可执行文件或某些中间形式等。所述计算机可读介质可以包括：能够携带所述计算机程序代码的任何实体或装置、记录介质、U盘、移动硬盘、磁碟、光盘、计算机存储器、只读存储器(ROM，ROM，Read-OnlyMemory)、随机存取存储器(RAM，Random Access Memory)以及软件分发介质等。

尽管结合优选实施方案具体展示和介绍了本发明，但所属领域的技术人员应该明白，在不脱离所附权利要求书所限定的本发明的精神和范围内，在形式上和细节上可以对本发明做出各种变化，均为本发明的保护范围。

Claims (7)

1.一种机器人定位控制方法，其特征在于，包括以下步骤：

S1：采集不同时刻机器人的图像特征，构建小波Elman网络模型，将采集的图像特征作为模型的输入，系统定位的状态向量作为模型的输出，对模型的参数进行训练，以使模型输出的状态向量跟踪通过卡尔曼滤波算法预测的状态向量，将训练后的模型作为最终模型；

S2：将机器人的图像特征I(t)输入最终模型得到最终状态向量X₁(t)，将最终状态向量X₁(t)转换为微分矩阵G(t)，其中，x_n*m(t)＝g_nm(t)，x_n*m表示向量X₁(t)的第n×m个元素，g_nm表示矩阵G(t)的第n行中第m列的元素；

S3：根据微分矩阵G(t)计算机器人的控制量U^R(t)：

U^R(t)＝λΔtG(t)^T(G(t)G(t)^T)^-1I_e(t)+U^R(t-1)

I_e(t)＝I(t)-I^*

其中，上标T表示矩阵的转置，λ为控制系数，Δt表示时刻t和时刻t-1的时间差，I_e(t)为t时刻的图像特征误差，I^*为机器人的期望图像特征；

S4：根据机器人的控制量U^R(t)对机器人的运动进行控制。

2.根据权利要求1所述的机器人定位控制方法，其特征在于：步骤S1具体包括以下步骤：

S11：将t时刻的图像特征I(t)作为小波Elman网络模型的输入，模型输出机器人系统定位的次优状态向量X'(t)；

S12：设定系统的状态转移方程为：

其中，X(t)和X(t-1)分别为t时刻和t-1时刻的状态向量，Ψ(t/t-1)为t-1至t时刻的状态转移矩阵，为系统噪声；

令X(t-1)＝X'(t)，根据系统的状态转移方程计算获得系统的状态向量X(t)；

S13：根据t时刻系统的状态向量X(t)和系统的状态观测方程，计算t时刻的状态观测值Z(t)；

状态观测方程为：

Z(t)＝Φ(t)X(t)+ψ(t)

其中，Φ(t)为观测矩阵，ψ(t)为观测噪声；

S14：采用卡尔曼滤波算法，并根据系统的状态转移方程和状态观测方程，对系统定位状态进行预测，得到t时刻机器人系统定位的最优状态估计向量

S15：以图像特征I(t)为输入量，以步骤S14得到的最优状态估计向量为参考值，采用梯度下降法对t时刻小波Elman网络模型的权值进行调整，以实现小波Elman网络模型的输出X'(t)跟踪卡尔曼滤波算法的输出

3.根据权利要求1所述的机器人定位控制方法，其特征在于：小波Elman网络模型的构建方法为：

步骤1：输入层神经元将输入的图像特征传递至隐含层，输入层神经元的传输函数为线性函数，输入层每个神经元节点i的输入和输出分别为：

其中，和分别为输入层第i个节点的输入和输出值，上标表示层，下标i＝1,2,...,m⁽¹⁾表示神经元的序号，m⁽¹⁾表示输入层神经元的总数；

步骤2：关联层对隐含层输出和自身层输出进行反馈传输，关联层每个神经元节点j的输入和输出分别为：

其中，和分别为关联层第j个节点的输入和输出值，为隐含层的输出值，下标j＝1,2,...,m⁽²⁾表示神经元的序号，m⁽²⁾表示关联层和隐含层神经元总数，0≤γ≤1为关联层反馈增益；

步骤3：反馈层神经元对输出层实现非线性反馈传递，反馈层每个神经元节点k的输入和输出分别为：

其中，和分别为反馈层第k个节点的输入和输出值，为输出层的输出值，上标表示层，下标k＝1,2,...,m⁽³⁾表示神经元的序号，m⁽³⁾表示反馈层神经元总数，exp(-x²)＝e^-x2为神经元非线性激励函数；

步骤4：隐含层神经元激励函数采用小波函数，隐含层每个神经元节点l的输入和输出表示为：

h(x)＝-x exp[-x²/2]

其中，和分别为隐含层第l个节点的输入和输出值，a_l(t)为小波伸缩系数，b_l(t)为小波平移系数，上标表示层，下标l＝1,2,...,m⁽⁴⁾表示神经元的序号，m⁽⁴⁾表示神经元总数；

步骤5：输出层每个节点c的输入和输出表示为：

其中，和分别为输出层第c个节点的输入和输出值，上标表示层，下标c＝1,2,...,m⁽⁵⁾表示神经元的序号，m⁽⁵⁾表示神经元总数，X′(t)为机器人系统定位的次优状态向量。

4.根据权利要求2所述的机器人定位控制方法，其特征在于：步骤S14的系统定位状态预测过程包括：

步骤1：初始化设置：

其中，G(0)和X(0)分别表示初始状态的微分矩阵和状态向量，Q和R均为噪声方差矩阵；

步骤2：计算中间状态向量

步骤3：预测误差协方差P(t/t-1)：P(t/t-1)＝Ψ(t/t-1)P(t-1)Ψ(t/t-1)^T+Q；

步骤4：计算卡尔曼滤波增益K(t)：K(t)＝P(t/t-1)Φ(t)^T(Φ(t)P(t/t-1)Φ(t)^T+R)^-1；

步骤5：计算t时刻机器人系统定位最优状态估计向量

5.根据权利要求2所述的机器人定位控制方法，其特征在于：步骤S15中权值w_l(t)更新规则为：

其中，η表示学习率。

6.一种机器人定位控制终端设备，其特征在于：包括处理器、存储器以及存储在所述存储器中并在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现如权利要求1～5中任一所述方法的步骤。

7.一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现如权利要求1～5中任一所述方法的步骤。