一种全向调节质心校正估计位姿的履带底盘及其控制方法技术

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

【技术实现步骤摘要】
一种全向调节质心校正估计位姿的履带底盘及其控制方法


[0001]本专利技术属于智能农机履带底盘控制领域，涉及一种全向调节质心校正估计位姿的农机履带底盘，本专利技术同时还涉及该农机履带底盘的控制方法。

技术介绍

[0002]我国丘陵山地面积占我国土地总面积的69.4%，有着极其丰富的自然资源，但GDP仅占全国的30%。丘陵山地地区海拔高度高于500米后，地形起伏较大、坡度陡峻、地表形态多样，地形地貌复杂多变，地理条件较差，田块面积较小，因此农机发展水平严重滞后。开展适于丘陵山区农田地形复杂、种植模式多元化、作物品种多样的通用动力底盘和轻简型作业机具，已成为当下以及相当长时期内推进丘陵山区农机化和农业现代化的重要且迫切的任务。
[0003]综合考虑行走稳定性、路面适应性以及经济性等方面，相较于轮式底盘，履带式底盘具有接地比压小、转向灵活、机动性好等优异性能，更适用于丘陵山地。国外对调姿底盘研究起步较早，产品在丘陵山地的通过性和稳定性更好。目前国内也研发出具有横向调姿功能的倒梯形履带底盘，但机体在纵坡行驶的易倾覆问题还未解决。

技术实现思路

[0004]本专利技术的目的是针对现有履带底盘无法适用丘陵山地复杂地况的缺陷，提供一种全向调节质心校正估计位姿的农机履带底盘。
[0005]本专利技术的另一目的是提供一种上述农机履带底盘的控制方法，实现履带底盘全向质心调整与横向位姿预估计与实时更新，有效提升底盘对地形的适应能力与自动运行能力。
[0006]为实现其目的，本专利技术采用如下技术方案：一种全向调节质心校正估计位姿的农机履带底盘，包括对称设于机架顶部底面上农机行走方向前端的第一升降油缸和第二升降油缸，以及对称设于机架顶部底面上农机行走方向后端的第三升降油缸和第四升降油缸；机架顶部还分别设有控制器、倾角传感器和位移传感器，所述倾角传感器包括x轴倾角传感器和y轴倾角传感器；所述机架底部上表面农机行走方向后端设有张紧油缸，张紧油缸连接张紧轮，机架底部上表面农机行走方向前端设有驱动轮，机架底部上表面中心位置设有托带轮，机架底部中间设有调平机构；所述调平机构包括底盘左右两侧的横向调节油缸、主动调平拨叉、顶端与机架底部下表面铰接的若干从动调平拨叉，以及数目与主动调平拨叉和从动调平拨叉总和相等的摇杆；所述横向调节油缸尾部与机架底部铰接，横向调节油缸活塞杆端与主动调平拨叉一端铰接，主动调平拨叉中部与机架底部铰接，所述主动调平拨叉另一端以及从动调平拨叉底端分别与对应的摇杆一端连接，摇杆上均设有锁止机构，摇杆另一端与断开式支重轮安装架铰接，断开式支重轮安装架上等距安装有若干支重轮，所述主动调平拨叉还通过连杆
与相邻的一个从动调平拨叉底端相接，所述锁止机构、主动调平拨叉、连杆和摇杆通过齿轮轴连接；机架同侧的所述张紧轮、托带轮、驱动轮和支重轮外周设有行走履带；所述控制器信号输入端分别与位移传感器、倾角传感器电连接，控制器信号输出端分别与横向调节油缸、张紧油缸、电动马达和四个升降油缸电连接。
[0007]作为本专利技术技术方案的进一步改进，所述锁止机构包括锁体，锁体顶部设有锁孔，锁孔内设有电动马达，电动马达通过其电枢轴与盘形凸轮固接；所述齿轮轴的齿轮部分两侧设有棘爪，棘爪与锁体之间装有弹簧，所述摇杆、连杆和锁体空套在齿轮轴上，主动调平拨叉的齿孔与齿轮轴齿轮部分相啮合。
[0008]本专利技术上述全向调节质心校正估计位姿的农机履带底盘的控制方法，包括全向质心调节与位姿校正估计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：建立状态转移矩阵进行状态一步预测；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方面情况对导航定位测速精度进行分析，它们具有以下的简单计算关系：。
[0009]进一步地，步骤S3中，底盘左右两侧横向调节油缸调整方法为：平坦地面行驶机身左倾，左侧横向调节油缸上升；平坦地面行驶机身右倾，右侧横向调节油缸上升；横坡仰坡行驶机身左倾，左侧横向调节油缸上升；横坡仰坡行驶机身右倾，左侧横向调节油缸下降；横坡俯坡行驶机身左倾，右侧横向调节油缸下降；横坡俯坡行驶机身右倾，右侧横向调节油缸上升。
[0010]步骤S5中，机架前后两组升降油缸调整方法为：平坦地面行驶机身前倾，第一升降油缸和第二升降油缸上升；平坦地面行驶机身后倾，第三升降油缸和第四升降油缸上升；纵坡上坡行驶机身前倾，第三升降油缸和第四升降油缸下降；纵坡上坡行驶机身后倾，第一升降油缸和第二升降油缸下降；纵坡下坡行驶机身前倾，第一升降油缸和第二升降油缸上升；纵坡下坡行驶机身后倾，第三升降油缸和第四升降油缸上升。
[0011]与现有本文档来自技高网...

【技术保护点】

【技术特征摘要】
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）上均设有锁止机构（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.如权利要求1所述的一种全向调节质心校正估计位姿的农机履带底盘，其特征在于，所述锁止机构（27）包括锁体（19），锁体（20）顶部设有锁孔，锁孔内设有电动马达（20），电动马达（20）通过其电枢轴与盘形凸轮（23）固接；所述齿轮轴（24）的齿轮部分两侧设有棘爪（22），棘爪（22）与锁体（19）之间装有弹簧（21），所述摇杆（16）、连杆（15）和锁体（19）空套在齿轮轴（24）上，主动调平拨叉（6）的齿孔与齿轮轴（24）齿轮部分相啮合。3.如权利要求1所述的一种全向调节质心校正估计位姿的农机履带底盘的控制方法，其特征在于，包括全向质心调节与位姿校正估计2个顺序控制流程：全向质心调节流程为S1
‑
S6：S1，判断机身行驶情况：分为纵坡上坡行驶、纵坡下坡行驶、横坡仰坡行驶、横坡俯坡行驶与平坦地幔行驶；S2，判断机身初始位姿：x轴倾角传感器检测机身横向倾翻角，y轴倾角传感器检测机身纵向翻滚角；S3，横向姿态调整：控制器（10）先判断机体底盘左右两侧横向调节油缸（5）初始位置和两侧横向调节油缸（5）间距B，通过位移传感器（9）实时检测横向调节油缸（5）移动距离，根据倾翻角与横向调节油缸（5）移动距离关系曲线输出活塞杆移动距离；结合机身行驶情况，控制器（10）控制底盘左右两侧横向调节油缸（5）动作；S4，机身沿x轴水平，底盘左右两侧横向调节油缸（5）...

【专利技术属性】
技术研发人员：吕凤玉，贺成柱，潘伟阳，丁立利，孙安，
申请(专利权)人：甘肃省机械科学研究院有限责任公司，
类型：发明
国别省市：