一种农用履带机器人滑动量的估计方法

摘要

本发明公开一种农用履带机器人滑动量的估计方法，通过农用履带机器人建模分析建立其运动方程，利用农用履带机器人上安装的GPS接收机接收的位置、速度信息，陀螺仪采集的方向信息以及加速度传感器接收的加速度信息，确定由这些信息决定的农用履带机器人的观测量Z_k＝[z_x,k，z_y,k，z_v,x，z_v,y，z_θ,k]^T，进而建立农用履带机器人的状态方程和观测方程：利用UKF将履带机器人状态方程和观测方程融合起来，获得准确的农用履带机器人姿态信息，再根据滑动参数计算方程，计算出滑动参数。本发明为农用履带机器人按其轨道正常工作提供保障。

主权项

一种农用履带机器人滑动量的估计方法，通过对农用履带机器人建模分析建立其运动方程，利用农用履带机器人上安装的GPS接收机接收的位置、速度信息，陀螺仪采集的方向信息以及加速度传感器接收的加速度信息，确定由这些信息决定的农用履带机器人的观测量Z_k＝[z_x,k，z_y,k，z_v,x，z_v,y，z_θ,k]^T，进而建立农用履带机器人的状态方程和观测方程：利用UKF将履带机器人状态方程和观测方程融合起来，获得准确的农用履带机器人姿态信息，再根据滑动参数计算方程，计算出滑动参数，其特征在于，具体包括以下步骤：(1)、采用瞬态运动分析法对农用履带机器人建模，选定所在的位置坐标系xoy和全局笛卡尔坐标XOY，且两坐标系之间的夹角为θ，并得到农用履带机器人的运动学方程：x^&＝v_xcosθ‑v_ysinθ  (3)y^&＝v_xsinθ+v_ycosθ  (4)其中，v_x、v_y为农用履带机器人平动速度v在xoy坐标系上的投影；ω_l、ω_r为农用履带机器人左右轮的角速度，r为履带机器人履带驱动轮半径，b是履带机器人两履带中心间距,L是履带长度；(2)、利用农用履带机器人上安装的GPS接收机接收的位置、速度信息，陀螺仪采集的方向信息以及加速度传感器接收的加速度信息，确定由这些信息决定的农用履带机器人的观测量Z_k＝[z_x,k，z_y,k，z_v,x，z_v,y，z_θ,k]^T，进而建立农用履带机器人的状态方程和观测方程：(a)、状态方程的建立：用X_k＝[x_k,y_k,v_x,v_y,θ_k]^T描述履带机器人的姿态，机器人离散非线性状态方程为X_k+1＝f(X_k)+W_k  (6)其中$f\left(X_k\right)=\left[\begin{array}{c}{x}_k+{\mathrm{tv}}_{x,k}\cos {\theta }_k-{\mathrm{tv}}_{y,k}\sin {\theta }_k\\ y_k+{\mathrm{tv}}_{x,k}\sin {\theta }_k+{\mathrm{tv}}_{y,k}\cos {\theta }_k\\ v_{x,k}+{\mathrm{tv}}_{y,k}{r}_k+{\mathrm{ta}}_{x,k}\\ v_{y,k}-{\mathrm{tv}}_{x,k}{r}_k+{\mathrm{ta}}_{y,k}\\ {\theta }_k+{\mathrm{tr}}_{m,k}\end{array}\right];$W_k＝[0 0 tw_a,x tw_a,y tw_r]^T式中W_k—噪声矢量；t—是采样周期；x_k,y_k—全局坐标系中X、Y轴的位置；θ_k—笛卡尔全局坐标系中运动方向与航向的偏差；v_x,k,v_y,k—x和y轴上速度；a_x,k,a_y,k—x和y轴上的加速度；r_m,k—陀螺仪测的瞬时偏航率；(b)、观测方程的建立：观测量与状态变量之间的观测方程为Z_k＝h_k+v_k  (7)其中$h_k=\left[\begin{array}{c}{x}_k\\ y_k\\ v_{x,k}\cos {\theta }_k-v_{y,k}\sin {\theta }_k\\ v_{x,k}\sin {\theta }_k+v_{y,k}\cos {\theta }_k\\ {\theta }_k\end{array}\right]$v_k＝[v_x,k v_y,k v_v,x v_v,y v_θ,k]^T式中v_k—观测噪声向量，其它参数定义同式(6)；(3)、利用UFK递推算法处理农用履带机器人离散的非线性状态方程与观测方程，融合更新农用履带机器人速度信息(4)、根据经UFK算法计算得到的农用履带机器人速度信息陀螺仪测量得到的瞬时偏航率r_k，以及左、右履带驱动轮角速度ω_L和ω_R，计算农用履带机器人的左、右履带滑动比和滑动角度如下：$\mathrm{il}=\frac{{\mathrm{r\omega }}_L-{\hat{v}}_x}{{\mathrm{r\omega }}_L}---\left(14\right)$$\mathrm{ir}=\frac{{\mathrm{r\omega }}_R-{\hat{v}}_x}{{\mathrm{r\omega }}_R}---\left(15\right)$$\delta =\arctan \left(\frac{0.5{\mathrm{Lr}}_k+v_y}{v_x}\right)-\gamma ---\left(16\right)$