[发明专利]基于容积卡尔曼滤波器的机械臂轨迹跟踪控制方法

说明书

本发明公开了一种基于容积卡尔曼滤波器的机械臂轨迹跟踪控制方法，具体包括3各部分，分别机械臂动力学方程的建立，运动轨迹的跟踪控制以及数值的仿真。本发明公开的一种基于容积卡尔曼滤波器的机械臂轨迹跟踪控制方法，在给定一个不够准确的参数化模型，通过数值仿真计算，表明文中方法的有效性，该方法具有对运动目标更强的跟踪能力,跟踪精度更高,均方误差更小。

技术领域

本发明涉及运动学领域领域，特别涉及一种基于容积卡尔曼滤波器的机械臂轨迹跟踪控制方法。

背景技术

机械臂已广泛应用在工业控制中，取代人工，执行单调动作以提升产能，它同时具有高效率及工作稳定两大优点，在追求自动化生产的过程中扮演不可或缺的角色。但实际中的机械臂目标调校，仍然普遍存在依靠工程师经验校准，以试误法完成。机械臂为复杂且非线性的多输入多输出系统，它具有时变、强耦合和非线性的动力学特性，其控制问题十分复杂，设计模式为基础的控制器难以控制此类系统。轨迹跟踪控制是在机器臂控制问题中的一个重要方面。轨迹跟踪控制要求机器臂的各个关节跟踪连续、时变的期望轨迹，期望轨迹往往比较复杂，速度、加速度参数突变时对于机械臂都存在一定的冲击性。由于机器臂系统本身的复杂程度较高以及所跟踪轨迹的连续时变性，机器臂轨迹跟踪控制难度十分大，控制精度也难以保证。

根据机械臂具有非线性特性这一特点，常采用扩展卡尔曼滤波器进行轨迹跟踪状态估计。但是由于扩展卡尔曼滤波器经过一阶线性化近似后，忽略了部分非线性特性，在初始误差较大时，存在估计效果急剧下降和滤波器收敛速度缓慢的问题，甚至发生漏失的现象。为了更好地满足机械臂轨迹跟踪控制要求，这里有必要研究精度更高、工程实现更简单的方法。

发明内容

本发明的目的是为了解决上述问题，提供一种基于容积卡尔曼滤波器的机械臂轨迹跟踪控制方法，具有更好的鲁棒性，抗干扰性。

为达到上述目的，本发明采用的方法是：基于容积卡尔曼滤波器的机械臂轨迹跟踪控制方法，包括如下步骤：

1、假定给定角度为(θ₁,θ₂)，在笛卡儿坐标下，机械臂的定位的表示法里表达式可以表示如下方程：

式(1)中，θ₁为上机械臂角位移,θ₂为下机械臂角位移，r₁为上机械臂中心距，r₂为下机械臂中心距；

2、考虑具有加性噪声的非线性系统状态方程和量测方程：

式(2)中，x_k∈Rⁿ是系统在k时刻的状态，u_k－1∈Rⁿ是系统的输入，y_k∈R^m是系统的量测值，Rⁿ是有限集。d_k和v_k为不相关零均值高斯白噪声，Q_k－1和R_k均为协方差阵，m，n，k为整数。

3、通过三阶容积积分法则，利用2n个容积点加权求和近似计算加权高斯积分，对于函数f(x)的加权高斯积分，采用容积卡尔曼滤波器算法进行机械臂轨迹跟踪控制。

4、令状态向量和量测向量分别为x＝[θ₁,θ₂]^T和y＝[y₁,y₂]^T，由(1)式转换可得机械臂的逆向运动学的状态方程，并离散化为：