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

摘要

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

主权项

1.基于容积卡尔曼滤波器的机械臂轨迹跟踪控制方法，其特征在于，包括如下步骤:(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)式转换可得机械臂的逆向运动学的状态方程，并离散化为：式(3)中，x_k+1为是系统在k+1时刻的状态，θ_1,k上机械臂角位移离散值，θ_2,k为下机械臂角位移离散值。将式(3)等效为式(2)，从而可以运用容积卡尔曼滤波器进行轨迹跟踪控制。