一种改进卡尔曼滤波坐标分离机械手控制算法的制作方法

本发明属于自动化控制领域，特别是一种基于卡尔曼滤波的机械手控制算法，具体为一种改进卡尔曼滤波坐标分离机械手控制算法。可应用于军事，勘探，家庭护理，药品检测等领域。

背景技术：

智能机器人技术涉及计算机应用技术、自动控制、人工智能、传感器、运动控制等众多学科。但是，怎样使机器人更加自主化和智能化已得到更广泛的应用领域是现代机器人研究面临的重大难题。应用智能机器人可以帮助老年人和残疾人来完成他们日常生活中难以做到的一些操作，解决他们生活上的困难。而在工业生产和军事领域中，机器人可以完成抓取搬运任务，可以避免在恶劣和危险环境中人工作业的危险性。

机械手的灵敏控制是智能机器人实现自动化非常关键的环节，在工业和军事等领域中，为实现精确控制手臂抓取动作，本发明提出一种改进卡尔曼滤波坐标分离机械手控制算法。所述算法具有很好的实时性、反馈性、安全性，并且提高了抓取目标物体的控制精度

技术实现要素：

本发明的目的是提供一种改进卡尔曼滤波坐标分离机械手控制算法，结合卡尔曼滤波算法，迭代算法以及坐标分离的方法提高机械手抓取目标物体的精度，增强控制过程中的可控性，减小累积误差以及人为误差。

卡尔曼滤波算法的核心思想是利用k-1时刻采集到的机械手位置信息，来估计k时刻机械手的位置信息。卡尔曼滤波的算法的优势在于，舍掉冗余的观测历史数据，只采集前一时刻的数据，累积误差会大幅度减少。系统中误差估计和均方差也在时刻改变，具有非常好的时变性。应用到机械手的控制中，系统的运算速度和控制精度会同时得到质的改变。

机械手的k时刻位置信息由角度传感器和加速度传感器采集，角度传感器噪声偏差为wk，加速度传感器噪声偏差为vk，机械手末端轨迹sk的状态方程和测量方程yk表示为：

s(k)＝as(k-1)+bu(k)+w(k)(1)

y(k)＝hs(k)+v(k)(2)

其中，a与b为系统参数；h为测量系统参数；u(k)是k时刻对系统的控制量，若系统没有控制量，则b＝0。

状态预测方程：

s(k/k-1)＝as(k-1/k-1)+bu(k)(3)

式中s(k/k-1)表示由k-1时刻预测k时刻的机械手轨迹，s(k-1/k-1)表示k-1时刻最优估计值。

预测均方差方程：

p(k/k-1)＝ap(k-1/k-1)a^t+q(4)

式中p(k-1/k-1)表示b(k-1/k-1)对应的协方差，q表示过程噪声协方差状态估计方程：

s(k/k)＝s(k/k-1)+ks(k)[yk-hs(k/k-1)](5)

式中为卡尔曼增益参数，卡尔曼滤波的最优增益方程为

ks(k)＝p(k/k-1)h^t/(hp(k/k-1)h^t+r)(6)

估计均方差方程：

p(k/k)＝(1-ks(k)h)p(k/k-1)(7)

经过方程式(3)-(7)的计算得出最优的状态估计值s(k/k)，然后将s(k/k)的最优状态估计值再次作为上一时刻的采集值，即s(k-1/k-1)。利用上述方程式重新代入，计算流程中过程协方差q和误差估计r都会根据新采集的数据值发生变化，满足系统的时变性。

卡尔曼滤波可以应用于线性系统和非线性系统，控制精度非常高，应用广泛，但是线性卡尔曼滤波器也不是万能的，它有一个短板就是发散。为了弥补这个缺点，这里采用迭代的控制算法使运算收敛，进一步提高控制精度。

机械手运动本身是一个连续性的动作，这里为了分析问题方便将其运动离散化，同时将运动时间分为等长的单位时间作为一个周期，在任意一个周期内机械手运算出现了范围内发散，结束本周期运算，立即重新进入下一个周期的动作。迭代运算律

sk+1(t)＝sk(t)-l(sk(t))ek(t)，k＝0，1，…，t∈[0，t](8)

其中ek(t)＝yk(t/t)-yk(t/t-1)为第k个周期的误差，l(sk(t))为线性学习算子。

由公式(8)和(9)可得

经过证明，当时k→∞，ek＞0，并且ek′＜0，则证明在迭代运算律中，ek是收敛的。同理将协方差和误差估计按同样的原理代入可使得卡尔曼滤波的发散问题得到很好地抑制。卡尔曼滤波和迭代运算的结合运算使得，机械手的控制运算精度和可靠性精度得到有力的保证。

坐标分离方法目的是考虑到在小车移动过程中，若机械手臂同时移动会出现在同一坐标空间的错位运算，对手臂的控制造成很大的干扰。采用特殊的坐标空间分离的方法。坐标分离算法的核心思想是将连续抓取的动作离散化，完整的动作分为等时间周期的单次动作。上一个时间周期末的手臂末端坐标通过卡尔曼迭代滤波算法得出的新值，作为下一个时间周期开始的手臂末端坐标，依次类推不断得出新值，反复利用卡尔曼迭代计算。每一个时间周期手臂末端坐标都是当前最新的状态。

附图说明

图1机械手臂与小车整体坐标系

图2机械手臂单独坐标系

图3机械手臂抓取目标物体坐标系

图4机械手臂控制的反馈控制流程图

具体实施方式

本发明结合附图如下：

图1示出了机械手臂与小车整体坐标系，这里假设既定的场所均属于理想状态。即被抓取的物体在空间上静止不动的，机械手臂是搭载在小车上，因此小车与机械手臂是一个整体将他们与被抓取的物体放在一个坐标系里面。

目标物体的位置方程：

p＝(x1，y1，z1)(11)

手臂整体的位置方程：

s＝(x′1，y′1，z′1)(12)

如果满足以下方程式

则说明小车可以停下来，手臂执行抓取动作。这里的l表示手臂在三维空间的能延长的最长距离。

图2示出了机械手臂单独坐标系。目的是考虑到在小车移动过程中，若机械手臂同时移动会出现在同一坐标空间的错位运算，对手臂的控制造成很大的干扰。采用特殊的坐标空间分离的方法。将手臂的动作看做单独一个坐标空间，由图2所示，并且定义了手臂动作的空间边界，实际采用的机械臂是4自由度，三个臂长分别为l1＝10cm，l2＝12cm，l3＝9cm，z轴方向的可达最大长度为25cm，y轴单向方向可达最大长度为22cm，由于手臂可绕z轴旋转，因此y轴双向方向可达最大长度44cm，x轴方向最大长度为20cm。由以上手臂尺寸及旋转自由度构成的空间是一个类似图3的封闭空间，如果被抓取物体在此空间内，则代表可抓取。如果被抓取物体超出此空间，小车继续移动追寻目标物体，小车移动过程中，机械手臂始终处于原点位置。

为了消除累积误差，机械手臂的坐标原点不是固定不变的。由方程(1)可知

s(k)＝as(k-1)+bu(k)+w(k)(14)

令s(k)＝a(xko，yko，zko)(15)

当机械臂完成一个周期的动作时，上一次的坐标原点清零，由机械臂最后一次的位置作为坐标原点，依次计算下去。

因此，在此算法中机械手臂自动抓取物体的过程，可以简化成机械手臂坐标原点逐渐趋近与目标物体坐标原点的过程。

图3示出了机械手臂抓取目标物体坐标系。若手臂末端处于o2的坐标位置，并且要抓取位于o1的目标物体。可令o2的坐标为(x0，y0，z0)，o1的坐标为(xn，yn，zn)，手臂抓取目标物体的过程即趋近的过程。将这一过程代入卡尔曼滤波方程，令方程(3)中的则更新后的状态预测方程为

预测均方差方程为

式(16)、(17)预测出了每一个时间周期末的手臂末端坐标。接下来计算下一个时间周期开始的手臂末端坐标，代入方程(5)、(7)分别得到

状态估计方程

状态均方差方程为

计算过程中i的值由n递减到0，即表示计算结束，手臂完成了抓取动作。

图4示出了机械手臂控制的反馈控制流程图

传感器采集机械手臂的角度和角加速度位置信息传输给单片机的存储芯片。存储芯片采集位置信息给单片机cpu，cpu结合卡尔曼滤波器处理位置信息，将数据依次代入公式(3)～(7)进行反复计算，得到的下一刻数据进行迭代计算，保证运算收敛。利用反馈因子r和q调节控制误差，得到的新的反馈因子传输给单片机的存储芯片，再次重复上述过程，保证整个控制过程处于可控误差范围内。