[发明专利]一种动态补偿机器人直线导轨运动误差的方法

权利要求书

1.一种动态补偿机器人直线导轨运动误差的方法，其特征在于，包括以下步骤：

S10、先验估算：基于运动学方程，得到机器人当前时刻在直线导轨上的状态向量估计值与其上一时刻在直线导轨上的状态向量最优值之间的关系式，以及该两者的协方差的关系式；

步骤S10中的先验估算具体包括以下步骤：

S11、基于运动学方程，得到机器人k时刻在直线导轨上的状态向量理想值x_k^*，

式中，P_k为机器人k时刻在直线导轨上的空间位置，v_k为机器人k时刻在直线导轨上的速度，a为加速度，t为时间，x_k-1^*为机器人k-1时刻的状态向量理想值，A_k为k时刻的先验状态系数矩阵，B_k为k时刻的控制矩阵，u_k为k时刻的控制向量；

S12、考虑到不确定外界干扰ω_k，假设ω_k服从高斯分布，其期望为0，于是公式(1)变形为x_k＝A_kx_k-1'+B_ku_k+ω_k (2)，

式中，x_k为机器人k时刻在直线导轨上的状态向量估计值，x_k-1'为机器人k-1时刻在直线导轨上的状态向量最优值；

S13、由公式(2)创建x_k的协方差c_k与x_k-1'的协方差c_k-1'之间的关系式，

式中，Q_k为过程激励噪声协方差，Q_k＝f(B_ku_k+ω_k)；

S20、后验估算：在机器人直线导轨上安装并标定位移传感器，所述位移传感器实时采集机器人在直线导轨上的实际位移值，并根据位移传感器的精度计算得到位移传感器每一时刻实际位移值的协方差；

步骤S20中的后验估算具体包括以下步骤：

S21、位移传感器的理想测量值m_k＝H_kx_k，x_k为等于机器人当前时刻在直线导轨上的状态向量估计值，H_k为转换矩阵，以使m_k和x_k的量纲一致；

S22、考虑到传感器的误差o_k，求得机器人k时刻在直线导轨上的实际位移值z_k＝H_kx_k+o_k(4)；

S23、根据位移传感器的精度计算得到机器人k时刻在直线导轨上的实际位移值的协方差R_k；

S30、将S10中机器人当前时刻在直线导轨上的状态向量估计值及其协方差、S20采集到的机器人当前时刻在直线导轨上的实际位移值及其协方差代入卡尔曼滤波算法中，计算得到机器人当前时刻在直线导轨上的状态向量最优值；

步骤S30中，由卡尔曼算法结论，将机器人k时刻在直线导轨上的状态向量估计值及其协方差、机器人k时刻在直线导轨上的实际位移值及其协方差代入卡尔曼滤波算法，得到下列式子：

x'_k＝x_k+k_k(z_k-H_kx_k) (5)

c'_k＝c_k-k_kH_kc_k (6)

式中，x'_k为机器人k时刻在直线导轨上的状态向量最优值，k_k为k时刻的卡尔曼增益，c'_k为x'_k的协方差，为H_k的转置；

S40、将S30中得到的机器人当前时刻在直线导轨上的状态向量最优值作为S10中机器人上一时刻在直线导轨上的状态向量最优值，重复步骤S10-S30，进行迭代，直至计算完成，得到机器人最终时刻在直线导轨上的精确位置；

S50、将S40中机器人最终时刻在直线导轨上的精确位置与预计到达的理论位置作差，进行误差补偿，使机器人达到理论位置。