CN219237200U - 一种全向调节质心校正估计位姿的履带底盘 - Google Patents

Abstract

本实用新型公开了一种全向调节质心校正估计位姿的履带底盘，履带底盘包括设于机架上农机行走方向前端的四个升降油缸；机架顶部分别设有控制器、倾角传感器和位移传感器；机架底部农机行走方向后端设有张紧油缸，张紧油缸连接张紧轮，机架底部上表面农机行走方向前端设有驱动轮，机架底部上表面中心位置设有托带轮，机架底部中间设有调平机构。该履带底盘在横坡与纵坡行走时可确保质心在一定水平范围内，提高对崎岖不平路面的适应能力，解决了我国丘陵山地生产条件复杂，无机可用的问题。通过对该底盘进行位姿调节和新位姿估计，可实现底盘质心位姿预校正估计与实时更新，有效提升了农机底盘对地形的适应能力与自动运行能力。

Description

一种全向调节质心校正估计位姿的履带底盘

技术领域

本实用新型属于智能履带底盘领域，涉及一种全向调节质心校正估计位姿的履带底盘。

背景技术

我国丘陵山地面积占我国土地总面积的69.4%，有着极其丰富的自然资源，但GDP仅占全国的30%。丘陵山地地区海拔高度高于500米后，地形起伏较大、坡度陡峻、地表形态多样，地形地貌复杂多变，地理条件较差，田块面积较小，因此农机发展水平严重滞后。开展适于丘陵山区农田地形复杂、种植模式多元化、作物品种多样的通用动力底盘和轻简型作业机具，已成为当下以及相当长时期内推进丘陵山区农机化和农业现代化的重要且迫切的任务。

综合考虑行走稳定性、路面适应性以及经济性等方面，相较于轮式底盘，履带式底盘具有接地比压小、转向灵活、机动性好等优异性能，更适用于丘陵山地。国外对调姿底盘研究起步较早，产品在丘陵山地的通过性和稳定性更好。目前国内也研发出具有横向调姿功能的倒梯形履带底盘，但机体在纵坡行驶的易倾覆问题还未解决。

实用新型内容

本实用新型的目的是针对现有履带底盘无法适用丘陵山地复杂地况的缺陷，提供一种全向调节质心校正估计位姿的履带底盘。

为实现其目的，本实用新型采用如下技术方案：

一种全向调节质心校正估计位姿的履带底盘，包括对称设于机架顶部底面上农机行走方向前端的第一升降油缸和第二升降油缸，以及对称设于机架顶部底面上农机行走方向后端的第三升降油缸和第四升降油缸；机架顶部还分别设有控制器、倾角传感器和位移传感器，所述倾角传感器包括x轴倾角传感器和y轴倾角传感器；所述机架底部上表面农机行走方向后端设有张紧油缸，张紧油缸连接张紧轮，机架底部上表面农机行走方向前端设有驱动轮，机架底部上表面中心位置设有托带轮，机架底部中间设有调平机构；

所述调平机构包括底盘左右两侧的横向调节油缸、主动调平拨叉、顶端与机架底部下表面铰接的若干从动调平拨叉，以及数目与主动调平拨叉和从动调平拨叉总和相等的摇杆；所述横向调节油缸尾部与机架底部铰接，横向调节油缸活塞杆端与主动调平拨叉一端铰接，主动调平拨叉中部与机架底部铰接，所述主动调平拨叉另一端以及从动调平拨叉底端分别与对应的摇杆一端连接，摇杆另一端与断开式支重轮安装架铰接，断开式支重轮安装架上等距安装有若干支重轮，所述主动调平拨叉还通过连杆与相邻的一个从动调平拨叉底端相接，所述锁止机构、主动调平拨叉、连杆和摇杆通过齿轮轴连接；机架同侧的所述张紧轮、托带轮、驱动轮和支重轮外周设有行走履带；

所述控制器信号输入端分别与位移传感器、倾角传感器电连接，控制器信号输出端分别与横向调节油缸、张紧油缸、电动马达和四个升降油缸电连接。

作为本实用新型技术方案的进一步改进，所述摇杆上均设有锁止机构，锁止机构与齿轮轴连接。

进一步地，所述锁止机构包括锁体，锁体顶部设有锁孔，锁孔内设有电动马达，电动马达通过其电枢轴与盘形凸轮固接；所述齿轮轴的齿轮部分两侧设有棘爪，棘爪与锁体之间装有弹簧，所述摇杆、连杆和锁体空套在齿轮轴上，主动调平拨叉的齿孔与齿轮轴齿轮部分相啮合。

本实用新型上述全向调节质心校正估计位姿的履带底盘的控制方法，包括全向质心调节与位姿校正估计2个顺序控制流程：

全向质心调节流程为S1-S6：

S1，判断机身行驶情况：分为纵坡上坡行驶、纵坡下坡行驶、横坡仰坡行驶、横坡俯坡行驶与平坦地幔行驶；

S2，判断机身初始位姿：x轴倾角传感器检测机身横向倾翻角

，y轴倾角传感器检测机身纵向翻滚角/>

；

S3，横向姿态调整：控制器先判断机体底盘左右两侧横向调节油缸初始位置和两侧横向调节油缸间距B，通过位移传感器实时检测横向调节油缸移动距离，根据倾翻角

与横向调节油缸移动距离/>

关系曲线/>

输出活塞杆移动距离；结合机身行驶情况，控制器控制底盘左右两侧横向调节油缸动作；

S4，机身沿x轴水平，底盘左右两侧横向调节油缸停止动作；

S5，纵向姿态调整：控制器先判断机体机架前后两组升降油缸初始位置和前后两组升降油缸间距，通过位移传感器实时检测两组升降油缸移动距离，根据翻滚角

与升降油缸移动距离/>

关系曲线/>

输出活塞杆移动距离；结合机身行驶情况，控制器控制机架前后两组升降油缸动作；

S6，机身沿y轴保持垂直，底盘机架前后两组升降油缸停止动作；

位姿校正估计流程为S7-S14：

S7：将调整后底盘质心位姿输入组合惯导系统，作为系统初始位姿，解算组合惯导系统姿态、位置、速度误差；

S8：建立组合惯导系统离散化后的状态方程为：

其中，X(k)是状态向量，

是状态转移矩阵，Z(k)是观测向量，H(k)是观测矩阵，/>

是系统噪声矩阵，V(k)是观测噪声向量，W(k)是系统噪声向量；

S9：建立状态转移矩阵进行状态一步预测；

是系统状态矩阵估计值，/>

是状态转移矩阵；

S10：建立一步预测均方差矩阵：

其中，

是一步预测均方差矩阵，/>

是系统状态矩阵估计值，/>

是状态转移矩阵，Q(k)是噪声方差矩阵，/>

是系统噪声矩阵；

S11：通过卡尔曼滤波器优化估计系统状态量：

其中，K_k是卡尔曼滤波增益，

是一步预测均方差矩阵，H_k是观测矩阵，R(k)是噪声方差矩阵；

S12：通过预测与修正相结合的方式计算实时状态估计值，纠正组合惯导系统姿态、速度和位置等信息并更新：

其中，

是系统状态矩阵估计值，K_k是卡尔曼滤波增益，/>

是观测向量估计值，H_k是观测矩阵；

S13：建立估计误差方差矩阵：

其中，P_k误差协方差矩阵，I是单位矩阵，K_k是卡尔曼滤波增益，H_k是观测矩阵，R(k)是噪声方差矩阵；

S14：通过精度因子DOP反应系统精度，实现对组合惯导系统的系统噪声与观测噪声的参数辨识；从几何G、位置P、水平H、垂直V以及时钟差T方面情况对导航定位测速精度进行分析，它们具有以下的简单计算关系：

。

进一步地，步骤S3中，底盘左右两侧横向调节油缸调整方法为：平坦地面行驶机身左倾，左侧横向调节油缸上升；平坦地面行驶机身右倾，右侧横向调节油缸上升；横坡仰坡行驶机身左倾，左侧横向调节油缸上升；横坡仰坡行驶机身右倾，左侧横向调节油缸下降；横坡俯坡行驶机身左倾，右侧横向调节油缸下降；横坡俯坡行驶机身右倾，右侧横向调节油缸上升。

步骤S5中，机架前后两组升降油缸调整方法为：平坦地面行驶机身前倾，第一升降油缸和第二升降油缸上升；平坦地面行驶机身后倾，第三升降油缸和第四升降油缸上升；纵坡上坡行驶机身前倾，第三升降油缸和第四升降油缸下降；纵坡上坡行驶机身后倾，第一升降油缸和第二升降油缸下降；纵坡下坡行驶机身前倾，第一升降油缸和第二升降油缸上升；纵坡下坡行驶机身后倾，第三升降油缸和第四升降油缸上升。

与现有技术相比，由于采用了上述技术方案，本实用新型的有益技术效果是：

1、本实用新型提供的全向调节质心校正估计位姿的履带底盘，在横坡与纵坡行走时可确保质心在一定水平范围内，提高对崎岖不平路面的适应能力，解决了我国丘陵山地生产条件复杂，无机可用的突出问题。推广应用于农田耕地、起垄、插秧、喷药、收获等作业环节，可以大幅提升作业精度，提高土地利用率，降低生产成本，增加丘陵山区经济效益。

2、本实用新型提供的全向调节质心校正估计位姿的履带底盘，可通过检测履带底盘所处环境的地形参数，基于底盘位姿数学模型分析实际位姿和理想位姿的差距，确定最优姿态调节控制方法，使履带底盘以最快速、最安全的方式运动到目标位姿。将履带底盘位姿调节和新位姿估计相结合，整体提高了履带底盘定位精度，提高了底盘控制的灵活性和实时性。

附图说明

图1为本实用新型全向调节质心校正估计位姿的履带底盘的结构示意图；

图2为本实用新型中锁止机构的主视图；

图3为本实用新型中锁止机构的半剖视图；

图4为断开式支重轮安装架轴侧视图；

图5为履带底盘横坡横向调平示意图；

图6为履带底盘纵坡上坡调平示意图；

图7为履带底盘横向调节控制方法示意图；

图8为履带底盘纵向调节控制方法示意图；

图9为履带底盘全向质心调节流程图；

图10为履带底盘位姿校正估计流程图；

其中，1、张紧轮；2、机架；3、张紧油缸；4、第三升降油缸；5、横向调节油缸；6、主动调平拨叉；7、托带轮；8、倾角传感器；9、位移传感器；10、控制器；11、第一升降油缸；12、驱动轮；13、行走履带；14、支重轮；15、连杆；16、摇杆；17、第四升降油缸；18、第二升降油缸；19、锁体；20、电动马达；21、弹簧；22、棘爪；23、盘形凸轮；24、齿轮轴；25、从动调平拨叉；26、断开式支重轮安装架；27、锁止机构。

具体实施方式

下面结合附图对本实用新型进行详细说明。

如图1-4所示，本发明提供的一种全向调节质心校正估计位姿的履带底盘，包括对称设于机架2顶部底面上农机行走方向前端的第一升降油缸11和第二升降油缸18，以及对称设于机架2顶部底面上农机行走方向后端的第三升降油缸4和第四升降油缸17；机架2顶部还分别设有控制器10、倾角传感器8和位移传感器9，所述倾角传感器8包括x轴倾角传感器和y轴倾角传感器；所述机架2底部上表面农机行走方向后端设有张紧油缸3，张紧油缸3连接张紧轮1，机架2底部上表面农机行走方向前端设有驱动轮12，机架2底部上表面中心位置设有托带轮7，机架2底部中间设有调平机构。所述调平机构包括底盘左右两侧的横向调节油缸5、主动调平拨叉6、顶端与机架2底部下表面铰接的若干从动调平拨叉25，以及数目与主动调平拨叉6和从动调平拨叉25总和相等的摇杆16；所述横向调节油缸5尾部与机架2底部铰接，横向调节油缸5活塞杆端与主动调平拨叉6一端铰接，主动调平拨叉6中部与机架2底部铰接，所述主动调平拨叉6另一端以及从动调平拨叉25底端分别与对应的摇杆16一端连接，摇杆16上均设有锁止机构27，摇杆16另一端与断开式支重轮安装架26铰接，断开式支重轮安装架26上等距安装有若干支重轮14，所述主动调平拨叉6还通过连杆15与相邻的一个从动调平拨叉25底端相接，所述锁止机构27、主动调平拨叉6、连杆15和摇杆16通过齿轮轴24连接；机架2同侧的所述张紧轮1、托带轮7、驱动轮12和支重轮14外周设有行走履带13；

所述控制器10信号输入端分别与位移传感器9、倾角传感器8电连接，控制器10信号输出端分别与横向调节油缸5、张紧油缸3、电动马达21和四个升降油缸电连接。

如图2-3所示，所述锁止机构27包括锁体19，锁体19顶部设有锁孔，锁孔内设有电动马达20，电动马达20通过其电枢轴与盘形凸轮23固接；所述齿轮轴24的齿轮部分两侧设有棘爪22，棘爪22与锁体19之间装有弹簧21，所述摇杆16、连杆15和锁体19空套在齿轮轴24上，主动调平拨叉6的齿孔与齿轮轴24齿轮部分相啮合。

底盘横坡调节工作原理如图5所示，当机体在横坡行驶时，控制器10接收到倾角传感器8信号后发出指令给锁止机构27中电动马达20令其转动，电动马达20电枢轴带动盘形凸轮23顺时针转动90°，克服弹簧21作用力将棘爪22向外转动，此时锁止机构27处于开锁状态，同时，横向调节油缸5收缩并带动主动调平拨叉6上端绕中间固定点转动，此时主动调平拨叉6和摇杆16之间角度逐渐变大，通过连杆15带动从动调平拨叉25同规律运动，推动断开式支重轮安装架26及支重轮14组下移，由于支重轮14组时刻与地面接触，来自地面的反作用力将倾斜侧机架2顶起，当倾角传感器8检测到整机处于水平状态时，横向调节油缸5停止动作，锁止机构27中电动马达20带动盘形凸轮23再次顺时针转动90°，在弹簧21的作用下推动棘爪22前端向内转动，直至棘爪22上表面与齿轮轴24的齿轮部分贴合，锁止机构27此时处于锁止状态。锁止机构27的作用一是由于主动调平拨叉6、摇杆16和断开式支重轮安装架26三者之间均可相对转动，锁止机构27可防止调平后主动调平拨叉6与摇杆16之间的角度发生变化影响调平精度；二是减少横向调节油缸5承受的来自路面的反冲击力。为实现行走履带13张紧力始终能够保持在预设范围内，横向调节油缸5收缩的同时，张紧油缸3也随之收缩带动张紧轮1后移。此时，横向重心调整过程结束，重心恢复到初始位置附近。

底盘纵坡调节工作原理如图6所示，机体纵坡行驶时，机身发生前后倾，根据y轴倾角传感器的信号，机架2前后侧四个升降油缸的推杆伸缩带动机架2动作，四个升降油缸共同抬起或推低机架2，实现前后侧升降油缸组具有一定高度差，进而使机体y轴向达到接近水平甚至水平状态。底盘平坦地面行驶时，x轴倾角传感器和y轴倾角传感器同时输出x与y轴向信号，底盘左右两侧横向调节油缸5、机架2下方四个升降油缸的推杆伸出的部分全部收回，直到机体前后左右全方位接近水平甚至达到水平状态。

如图7-10所示，本实用新型所述全向调节质心校正估计位姿的履带底盘的控制方法，包括全向质心调节与位姿校正估计2个顺序控制流程：

全向质心调节流程为S1-S6：

S1，判断机身行驶情况：分为纵坡上坡行驶、纵坡下坡行驶、横坡仰坡行驶、横坡俯坡行驶与平坦地幔行驶；

S2，判断机身初始位姿：x轴倾角传感器检测机身横向倾翻角

，y轴倾角传感器检测机身纵向翻滚角/>

；

S3，横向姿态调整：控制器10先判断机体底盘左右两侧横向调节油缸5初始位置和两侧横向调节油缸5间距B，通过位移传感器9实时检测横向调节油缸5移动距离，根据倾翻角

与横向调节油缸5移动距离/>

关系曲线/>

输出活塞杆移动距离；结合机身行驶情况，控制器10控制底盘左右两侧横向调节油缸5动作；底盘左右两侧横向调节油缸5调整方法为：平坦地面行驶机身左倾，左侧横向调节油缸5上升；平坦地面行驶机身右倾，右侧横向调节油缸5上升；横坡仰坡行驶机身左倾，左侧横向调节油缸5上升；横坡仰坡行驶机身右倾，左侧横向调节油缸5下降；横坡俯坡行驶机身左倾，右侧横向调节油缸5下降；横坡俯坡行驶机身右倾，右侧横向调节油缸5上升。

S4，机身沿x轴水平时，底盘左右两侧横向调节油缸5停止动作；

S5，纵向姿态调整：控制器10先判断机体机架2前后两组升降油缸初始位置和前后两组升降油缸间距，通过位移传感器9实时检测两组升降油缸移动距离，根据翻滚角

与升降油缸移动距离/>

关系曲线/>

输出活塞杆移动距离；结合机身行驶情况，控制器10控制机架2前后两组升降油缸动作；机架2前后两组升降油缸调整方法为：平坦地面行驶机身前倾，第一升降油缸11和第二升降油缸18上升；平坦地面行驶机身后倾，第三升降油缸4和第四升降油缸17上升；纵坡上坡行驶机身前倾，第三升降油缸4和第四升降油缸17下降；纵坡上坡行驶机身后倾，第一升降油缸11和第二升降油缸18下降；纵坡下坡行驶机身前倾，第一升降油缸11和第二升降油缸18上升；纵坡下坡行驶机身后倾，第三升降油缸4和第四升降油缸17上升；

S6，机身沿y轴保持垂直，底盘机架2前后两组升降油缸停止动作；

位姿校正估计流程为S7-S14：

S7：将调整后底盘质心位姿输入组合惯导系统，作为系统初始位姿，解算组合惯导系统姿态、位置、速度误差；

S8：建立组合惯导系统离散化后的状态方程为：

其中，X(k)是状态向量，

是状态转移矩阵，Z(k)是观测向量，H(k)是观测矩阵，/>

是系统噪声矩阵，V(k)是观测噪声向量，W(k)是系统噪声向量；

S9：建立状态转移矩阵进行状态一步预测；

是系统状态矩阵估计值，/>

是状态转移矩阵；

S10：建立一步预测均方差矩阵：

其中，

是一步预测均方差矩阵，/>

是系统状态矩阵估计值，/>

是状态转移矩阵，Q(k)是噪声方差矩阵，/>

是系统噪声矩阵；

S11：通过卡尔曼滤波器优化估计系统状态量：

其中，K_k是卡尔曼滤波增益，

是一步预测均方差矩阵，H_k是观测矩阵，R(k)是噪声方差矩阵；

S12：通过预测与修正相结合的方式计算实时状态估计值，纠正组合惯导系统姿态、速度和位置等信息并更新：

其中，

是系统状态矩阵估计值，K_k是卡尔曼滤波增益，/>

是观测向量估计值，H_k是观测矩阵；

S13：建立估计误差方差矩阵：

其中，P_k误差协方差矩阵，I是单位矩阵，K_k是卡尔曼滤波增益，H_k是观测矩阵，R(k)是噪声方差矩阵；

S14：通过精度因子DOP反应系统精度，实现对组合惯导系统的系统噪声与观测噪声的参数辨识；从几何G、位置P、水平H、垂直V以及时钟差T方面情况对导航定位测速精度进行分析，它们具有以下的简单计算关系：

。

综上，通过检测履带底盘所处环境的地形参数，基于底盘位姿数学模型分析实际位姿和理想位姿的差距，确定最优姿态调节控制方法，使履带底盘以最快速、最安全的方式运动到目标位姿。将履带底盘位姿调节和新位姿估计相结合，整体提高了履带底盘定位精度，提升了底盘控制的灵活性和实时性。

Claims (3)

1.一种全向调节质心校正估计位姿的履带底盘，其特征在于，包括对称设于机架（2）顶部底面上农机行走方向前端的第一升降油缸（11）和第二升降油缸（18），以及对称设于机架（2）顶部底面上农机行走方向后端的第三升降油缸（4）和第四升降油缸（17）；

机架（2）顶部还分别设有控制器（10）、倾角传感器（8）和位移传感器（9），所述倾角传感器（8）包括x轴倾角传感器和y轴倾角传感器；所述机架（2）底部上表面农机行走方向后端设有张紧油缸（3），张紧油缸（3）连接张紧轮（1），机架（2）底部上表面农机行走方向前端设有驱动轮（12），机架（2）底部上表面中心位置设有托带轮（7），机架（2）底部中间设有调平机构；

所述调平机构包括底盘左右两侧的横向调节油缸（5）、主动调平拨叉（6）、顶端与机架（2）底部下表面铰接的若干从动调平拨叉（25），以及数目与主动调平拨叉（6）和从动调平拨叉（25）总和相等的摇杆（16）；所述横向调节油缸（5）尾部与机架（2）底部铰接，横向调节油缸（5）活塞杆端与主动调平拨叉（6）一端铰接，主动调平拨叉（6）中部与机架（2）底部铰接，所述主动调平拨叉（6）另一端以及从动调平拨叉（25）底端分别与对应的摇杆（16）一端连接，摇杆（16）另一端与断开式支重轮安装架（26）铰接，断开式支重轮安装架（26）上等距安装有若干支重轮（14），所述主动调平拨叉（6）还通过连杆（15）与相邻的一个从动调平拨叉（25）底端相接，所述主动调平拨叉（6）、连杆（15）和摇杆（16）通过齿轮轴（24）连接；机架（2）同侧的所述张紧轮（1）、托带轮（7）、驱动轮（12）和支重轮（14）外周设有行走履带（13）；

所述控制器（10）信号输入端分别与位移传感器（9）、倾角传感器（8）电连接，控制器（10）信号输出端分别与横向调节油缸（5）、张紧油缸（3）、电动马达（20）和四个升降油缸电连接。

2.如权利要求1所述的一种全向调节质心校正估计位姿的履带底盘，其特征在于，所述摇杆（16）上均设有锁止机构，锁止机构与齿轮轴（24）连接。

3.如权利要求2所述的一种全向调节质心校正估计位姿的履带底盘，其特征在于，所述锁止机构包括锁体（19），锁体（19）顶部设有锁孔，锁孔内设有电动马达（20），电动马达（20）通过其电枢轴与盘形凸轮（23）固接；所述齿轮轴（24）的齿轮部分两侧设有棘爪（22），棘爪（22）与锁体（19）之间装有弹簧（21），所述摇杆（16）、连杆（15）和锁体（19）空套在齿轮轴（24）上，主动调平拨叉（6）的齿孔与齿轮轴（24）齿轮部分相啮合。

Images