一种爬行焊接机器人和焊缝间夹角估计方法与流程

技术特征：

1.一种爬行焊接机器人和焊缝间夹角估计方法，其特征在于：包括如下步骤：

步骤一：获取传感器采集的信息；传感器包括激光焊缝跟踪传感器和陀螺仪，信息具体包括激光焊缝跟踪传感器采集的焊缝楞的焊缝楞的坐标，陀螺仪测量的机器人角速度信息；

获得k时刻的焊缝楞的坐标X_1m(k)和X_2m(k)，陀螺仪测量的机器人角速度ω_m(k)；

步骤二：获取激光焊缝跟踪传感器的运动信息；

获得k时刻的机器人前进速度V_C(k)，码盘计算出的机器人转角Δθ_m(k)；

步骤三：根据焊缝楞的坐标、机器人角速度以及前进速度和转角，建立状态方程，使用卡尔曼滤波器对爬行焊接机器人与焊缝间夹角进行最优估计，并得到焊缝楞的坐标的滤波值；

步骤四：重复上述步骤，不断地得到新的结果，直到焊接过程结束。

2.根据权利要求1所述的一种爬行焊接机器人和焊缝间夹角估计方法，其特征在于：步骤三中具体方法如下：

A.建立状态方程如下：

$\left[\begin{array}{c}\mathrm{\θ}\left(k\right)\\ \mathrm{\ω}\left(k\right)\\ X_1\left(k\right)\\ X_2\left(k\right)\\ \mathrm{\δ}\left(k\right)\end{array}\right]=\left[\begin{array}{ccccc}1& T& 0& 0& 0\\ 0& 1& 0& 0& 0\\ V_C\left(k\right)T& V_C\left(k\right)T^2/2& 1& 0& 0\\ V_C\left(k\right)T& V_C\left(k\right)T^2/2& 0& 1& 0\\ 0& 0& 0& 0& 1\end{array}\right]\left[\begin{array}{c}\mathrm{\θ}(k-1)\\ \mathrm{\ω}(k-1)\\ X_1(k-1)\\ X_2(k-1)\\ \mathrm{\δ}(k-1)\end{array}\right]+W$

$\left[\begin{array}{c}\mathrm{\Δ}{\mathrm{\θ}}_m\left(k\right)\\ {\mathrm{\ω}}_m\left(k\right)\\ X_{1m}\left(k\right)\\ X_{2m}\left(k\right)\end{array}\right]=\left[\begin{array}{ccccc}0& T& 0& 0& 0\\ 0& 1& 0& 0& 1\\ 0& 0& 1& 0& 0\\ 0& 0& 0& 1& 0\end{array}\right]\left[\begin{array}{c}\mathrm{\θ}\left(k\right)\\ \mathrm{\ω}\left(k\right)\\ X_1\left(k\right)\\ X_2\left(k\right)\\ \mathrm{\δ}\left(k\right)\end{array}\right]+V$

其中，T为采样时间，k为时刻，V_C为爬行焊接机器人前进速度，θ为爬行焊接机器人与焊缝间夹角，ω为爬行焊接机器人旋转角速度，X₁和X₂分别为焊缝第一、第二条楞的横坐标，δ为陀螺仪零偏，Δθ_m为通过码盘计算的爬行机器人转角，ω_m为陀螺仪测量的角速度，X_1m和X_2m分别为激光焊缝跟踪传感器测量的焊缝第一、第二条楞的横坐标，W为系统误差矩阵，V为测量误差矩阵；

将上式简记为：

X(k)＝Φ(k,k-1)X(k-1)+W

Z(k)＝HX(k)+V

其中，X为状态值向量，Φ为状态转移矩阵，Z为观测值向量，H为观测矩阵，其值分别为：

$X\left(k\right)=\left[\begin{array}{c}\mathrm{\θ}\left(k\right)\\ \mathrm{\ω}\left(k\right)\\ X_1\left(k\right)\\ X_2\left(k\right)\\ \mathrm{\δ}\left(k\right)\end{array}\right]$

$\mathrm{\Φ}(k,k-1)=\left[\begin{array}{ccccc}1& T& 0& 0& 0\\ 0& 1& 0& 0& 0\\ V_C\left(k\right)T& V_C\left(k\right)T^2/2& 1& 0& 0\\ V_C\left(k\right)T& V_C\left(k\right)T^2/2& 0& 1& 0\\ 0& 0& 0& 0& 1\end{array}\right]$

$Z\left(k\right)=\left[\begin{array}{c}\mathrm{\Δ}{\mathrm{\θ}}_m\left(k\right)\\ {\mathrm{\ω}}_m\left(k\right)\\ X_{1m}\left(k\right)\\ X_{2m}\left(k\right)\end{array}\right]$

$H=\left[\begin{array}{ccccc}0& T& 0& 0& 0\\ 0& 1& 0& 0& 1\\ 0& 0& 1& 0& 0\\ 0& 0& 0& 1& 0\end{array}\right]$

B.使用卡尔曼滤波器进行滤波，递推得到k时刻的状态值向量的估计值

$\hat{X}\left(k\right|k-1)=\mathrm{\Φ}(k,k-1)\hat{X}(k-1|k-1)$

P(k|k-1)＝Φ(k,k-1)P(k-1|k-1)Φ^T(k,k-1)+Q

K(k)＝P(k|k-1)H^T(HP(k|k-1)H^T+R)^-1

P(k|k)＝(I-K(k)H)P(k|k-1)

$\hat{X}\left(k\right|k)=\hat{X}\left(k\right|k-1)+K\left(k\right)\left(Z\right(k)-H\hat{X}(k|k-1\left)\right)$

其中，Q为系统误差W的协方差矩阵，R为测量误差V的协方差矩阵，为状态值向量的估计值，P为状态值向量的估计值的噪声协方差矩阵，I为单位矩阵，K为增益矩阵；

C.得到k时刻的爬行焊接机器人与焊缝间夹角估计值和滤波后的焊缝楞的横坐标及

3.根据权利要求2所述的一种爬行焊接机器人和焊缝间夹角估计方法，其特征在于：建立的矩阵形式的状态方程展开后，包括如下方程：

$X_n\left(k\right)=V_C\left(k\right)T\·\mathrm{\θ}(k-1)+\frac{1}{2}{\mathrm{VT}}^2\mathrm{\ω}(k-1)+X_n(k-1)$

其中，n代表焊缝的第n条楞，且不包括噪声项。