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

权利要求书

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)，从而可以运用容积卡尔曼滤波器进行轨迹跟踪控制。

2.根据权利要求1所述的基于容积卡尔曼滤波器的机械臂轨迹跟踪控制方法，其特征在于：式(3)中，状态方程在高斯噪音d～N(0,diag[0.01,0.1])，量测方程在量测噪音v∈N(0,0.005I)的扰动下给出结果，I为单位矩阵，diag为对角矩阵。

3.根据权利要求1所述的基于容积卡尔曼滤波器的机械臂轨迹跟踪控制方法，其特征在于：上机械臂和下机械臂的角度θ₁和θ₂均在一定的范围内移动，具体为θ₁∈[0.4,1.8]和θ₂∈[π/2,3π/2]。