CN116395027A - 一种基于stm32的轮式农机自动转向方法、系统及农机 - Google Patents
一种基于stm32的轮式农机自动转向方法、系统及农机 Download PDFInfo
- Publication number
- CN116395027A CN116395027A CN202310493685.7A CN202310493685A CN116395027A CN 116395027 A CN116395027 A CN 116395027A CN 202310493685 A CN202310493685 A CN 202310493685A CN 116395027 A CN116395027 A CN 116395027A
- Authority
- CN
- China
- Prior art keywords
- wheel
- steering
- angle
- agricultural machinery
- agricultural machine
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 49
- 238000005259 measurement Methods 0.000 claims abstract description 24
- 238000004364 calculation method Methods 0.000 claims abstract description 16
- 230000004044 response Effects 0.000 claims abstract description 11
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 7
- 230000001133 acceleration Effects 0.000 claims description 7
- 239000011159 matrix material Substances 0.000 claims description 7
- 229910000831 Steel Inorganic materials 0.000 claims description 6
- 239000010959 steel Substances 0.000 claims description 6
- 230000009466 transformation Effects 0.000 claims description 6
- 230000008569 process Effects 0.000 abstract description 6
- 238000009434 installation Methods 0.000 abstract description 4
- 238000001914 filtration Methods 0.000 description 8
- 238000005070 sampling Methods 0.000 description 8
- 238000010586 diagram Methods 0.000 description 6
- 230000006870 function Effects 0.000 description 6
- 230000010355 oscillation Effects 0.000 description 6
- 238000009825 accumulation Methods 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000005540 biological transmission Effects 0.000 description 1
- 238000004590 computer program Methods 0.000 description 1
- 238000010276 construction Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000002474 experimental method Methods 0.000 description 1
- 239000010720 hydraulic oil Substances 0.000 description 1
- 230000010354 integration Effects 0.000 description 1
- 238000012423 maintenance Methods 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 230000007246 mechanism Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 230000000737 periodic effect Effects 0.000 description 1
- 238000002407 reforming Methods 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 238000007142 ring opening reaction Methods 0.000 description 1
Images
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B62—LAND VEHICLES FOR TRAVELLING OTHERWISE THAN ON RAILS
- B62D—MOTOR VEHICLES; TRAILERS
- B62D6/00—Arrangements for automatically controlling steering depending on driving conditions sensed and responded to, e.g. control circuits
- B62D6/002—Arrangements for automatically controlling steering depending on driving conditions sensed and responded to, e.g. control circuits computing target steering angles for front or rear wheels
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B62—LAND VEHICLES FOR TRAVELLING OTHERWISE THAN ON RAILS
- B62D—MOTOR VEHICLES; TRAILERS
- B62D5/00—Power-assisted or power-driven steering
- B62D5/04—Power-assisted or power-driven steering electrical, e.g. using an electric servo-motor connected to, or forming part of, the steering gear
- B62D5/0457—Power-assisted or power-driven steering electrical, e.g. using an electric servo-motor connected to, or forming part of, the steering gear characterised by control features of the drive means as such
- B62D5/046—Controlling the motor
- B62D5/0463—Controlling the motor calculating assisting torque from the motor based on driver input
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02A—TECHNOLOGIES FOR ADAPTATION TO CLIMATE CHANGE
- Y02A40/00—Adaptation technologies in agriculture, forestry, livestock or agroalimentary production
- Y02A40/10—Adaptation technologies in agriculture, forestry, livestock or agroalimentary production in agriculture
Landscapes
- Engineering & Computer Science (AREA)
- Chemical & Material Sciences (AREA)
- Combustion & Propulsion (AREA)
- Transportation (AREA)
- Mechanical Engineering (AREA)
- Physics & Mathematics (AREA)
- Mathematical Physics (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Steering Control In Accordance With Driving Conditions (AREA)
Abstract
本发明属于农机自动驾驶辅助装置技术领域，具体涉及一种基于stm32的轮式农机自动转向方法、系统及农机；农机静止时，通过初始化程序测量转向系统响应死区，通过临界比例度法整定转向系统PID参数；农机作业时，转向控制系统在获取目标车轮转角后，通过姿态解算和卡尔曼最优估计对车轮偏转角进行非接触测量，通过前馈‑PID控制算法控制电动方向盘转动，实现对农机转向系统的闭环控制。本发明实现了车轮转角传感器的任意位置安装，降低了转向装置调试门槛，增强了对不同农机的适应性，在农机行进过程中最大程度消除轮式农机转向误差。
Description
技术领域
本发明属于农机自动驾驶辅助装置技术领域，具体涉及一种基于stm32的轮式农机自动转向方法、系统及农机。
背景技术
在农业规模化、集约化的过程中，精准农业的需求持续增长。农机自动驾驶技术是精准农业的核心领域，转向系统是自动驾驶的基础，对于轮式农机的自动驾驶作业相关技术的研究至关重要。当农机自动驾驶作业中，需要经常进行大角度调整或转弯调头，这对转向控制系统提出较高要求。转向轮作为农机转向的直接执行机构，对其的控制效果将直接影响农机实际作业性能。
当使用电动方向盘对传统农机进行电控化改造时，受传动结构中自由行程的影响，方向盘开环控制误差很大。为消除误差，一般采用角度传感器测量车轮转角进行闭环控制。这类传感器需要针对机型定制安装支架，并且在实际生产作业中容易损坏，稳定性较差。故开发有采用双陀螺仪组替代角度传感器的测量方式，分别将其水平安装于车架和车辆转向节上，保持车轮陀螺仪坐标系与车身基础坐标系一致，利用航向角差值估算车轮相对于车身的偏转角，但在实际操作中需要水平校准，对安装人员有较高的技术要求。同时在农机行进过程中，陀螺仪的累计误差会随时间增加而增加，测量精度下降，导致输出的转向角度漂移，转向系统闭环控制精度下降。
因此，基于上述技术问题需要设计一种新的基于stm32的轮式农机自动转向方法、系统及农机。
发明内容
本发明的目的是提供一种基于stm32的轮式农机自动转向方法、系统及农机。
为了解决上述技术问题，本发明提供了一种基于stm32的轮式农机自动转向方法，包括：
初次安装或维修调试时，农机静置，运行初始化程序；
通过姿态解算和卡尔曼最优估计进行车轮偏转角非接触式测量；
获取目标转角，计算转角偏差，控制电动方向盘转动。
进一步，所述初次安装或维修调试时，农机静置，运行初始化程序包括：
运行初始化程序，以转向轮直行中位为输入零点，输入电动方向盘的旋转角度，输出通过农机偏转角测量模块采集转向轮的旋转角度，以测量横轴零点两侧的响应死区。
进一步，通过临界比例度法整定PID参数KP,Ki,Kd。
进一步，所述通过传感器姿态解算和卡尔曼最优估计进行车轮偏转角的非接触式测量包括：
农机运行时，农机车轮偏转角测量模块初始化，每隔预设时间采样陀螺仪A的三轴角速度数据ωX、ωY、ωZ；
设置陀螺仪A的航向角角速度阈值，当ωZ大于角速度阈值时主控制器对此时采样的角速度进行姿态解算。
分别利用姿态角变换和四元数表示从载体坐标系到基础坐标系的转换矩阵，通过反解求出传感器A相对于农机基础坐标系OXYZ的航向角ψA。
进一步，若车轮转动速率
不为零时，主控制器通过姿态传感器B获取y轴方向上的加速度ay和车身转动的航向角ψB，采用阿卡曼转向模型，预估农机车轮转向角/>
进一步，根据
与/>
获得农机右前轮相对于车身的转向角速度/>
陀螺仪的累计误差为βk，对右前轮偏转角度
与βk的差做积分，得到右前轮转向角度估计值/>
进一步，主控制器以右前轮转向角度估计值
为角度测量系统状态量，以农机车轮转向角/>
为角度测量系统观测量，通过卡尔曼滤波器单元对车轮偏转角θfinal进行最优估计。
进一步，所述根据实际转向角度实时计算转角偏差，以调整电动方向盘转动角度包括：
把实际转向角度θfinal传递给主控制器，计算转角偏差，通过前馈-PID控制算法，实时调整车轮目标转动角度值uk。
第二方面，本发明还提供一种采用上述基于stm32的轮式农机自动转向方法的转向系统，包括：
初始化模块，初次安装或维修调试时，农机静置，运行初始化程序；
测量模块，通过姿态解算和卡尔曼最优估计进行车轮偏转角的非接触式测量；
调整模块，根据实际转向角度实时计算转角偏差，以调整电动方向盘转动角度。
第三方面，本发明还提供一种采用上述基于stm32的轮式农机自动转向方法的农机，包括：
主控制器、姿态传感器、陀螺仪和电动方向盘；
所述电动方向盘安装在农机车身转向立轴上；
所述姿态传感器安装在农机车身钢架上；
所述陀螺仪安装在车轮上任意随车轮转动的位置；
所述主控制器适于采用如权利要求1所述的基于stm32的轮式农机自动转向方法对电动方向盘进行控制。
本发明的有益效果是，本发明通过控制柜、电动方向盘、陀螺仪和姿态传感器构成，stm32主控芯片工控板(主控制器)置于控制柜中，电动方向盘代替原有方向盘安装于转向立轴上，姿态传感器安装在车身钢架上任意位置，陀螺仪安装在车轮上任意随车轮转动的位置。该系统运行包含以下步骤：农机静止时，通过初始化程序测量转向系统响应死区，通过临界比例度法整定PID参数；农机作业时，转向控制系统在获取目标车轮转角后，通过姿态解算和卡尔曼滤波对车轮偏转角进行非接触测量，通过前馈-PID控制算法控制电动方向盘转动，实现对农机转向系统的闭环控制。本发明实现了车轮转角传感器的任意位置安装，降低了转向装置调试门槛，增强了对不同农机的适应性，在农机行进过程中最大程度消除轮式农机转向误差。
本发明的其他特征和优点将在随后的说明书中阐述，并且，部分地从说明书中变得显而易见，或者通过实施本发明而了解。本发明的目的和其他优点在说明书以及附图中所特别指出的结构来实现和获得。
为使本发明的上述目的、特征和优点能更明显易懂，下文特举较佳实施例，并配合所附附图，作详细说明如下。
附图说明
为了更清楚地说明本发明具体实施方式或现有技术中的技术方案，下面将对具体实施方式或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施方式，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1是本发明的基于stm32的轮式农机自动转向方法的流程图；
图2是本发明的基于stm32的轮式农机自动转向方法的原理框图；
图3是本发明的角度AD值的临界振荡示意图；
图4是本发明的农机的结构示意图；
图5是本发明的基于stm32的轮式农机自动转向方法的具体流程图；
图6是本发明的轮式农机车轮偏转角测量模块安装俯视图；
图7是本发明的卡尔曼滤波运行方法流程图。
图中：
1控制柜、2电动方向盘3、姿态传感器B、4陀螺仪A。
具体实施方式
为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合附图对本发明的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
实施例1
如图1至图7所示，本实施例1提供了一种基于stm32的轮式农机自动转向方法，包括：步骤一，在农机静置时，运行初始化程序；步骤二，农机运行时，通过姿态解算和卡尔曼最优估计进行车轮偏转角非接触式测量；步骤三，获取目标车轮偏转角，以调整电动方向盘2转动角度；通过控制柜1、电动方向盘2、陀螺仪和姿态传感器构成，stm32主控芯片工控板(主控制器)置于控制柜1中，电动方向盘2代替原有方向盘安装于转向立轴上，姿态传感器安装在车身钢架上任意位置，陀螺仪安装在车轮上任意随车轮转动的位置。该系统运行包含以下步骤：在初次安装或维修调试时，农机静置，通过初始化程序测量转向系统响应死区，通过临界比例度法整定PID参数；农机作业时(农机运行时)，转向控制系统在获取目标车轮转角后，通过姿态解算和卡尔曼滤波对车轮偏转角进行非接触测量，通过前馈-PID控制算法控制电动方向盘2转动，实现对农机转向系统的闭环控制。实现了车轮转角传感器的任意位置安装，降低了转向装置调试门槛，增强了对不同农机的适应性，在农机行进过程中最大程度消除轮式农机转向误差。
在本实施例中，控制柜1安装于驾驶室右侧，内置stm32主控芯片嵌入式工控板(主控制器)，通过I2C接口与传感器相连；电动方向盘2代替原有方向盘安装于转向立轴上；姿态传感器B3安装在车身钢架上任意位置；陀螺仪A4安装在车轮上任意随车轮转动的位置。
在本实施例中，所述在农机静置时，运行初始化程序包括：农机静置时，运行初始化程序，以转向轮直行中位为输入零点，输入电动方向盘2的旋转角度(步进取为10°)，输出通过农机偏转角测量模块采集转向轮的旋转角度(稳态值，并且受机械结构限制存在左右限度)，以测量横轴零点两侧的响应死区；
设置条件前馈控制：
式中，F(s)为条件前馈控制函数；Δθ为转向轮预计旋转角度；k为PID控制的采样序列号。
在本实施例中，通过临界比例度法整定PID参数KP,Ki,Kd；在上述实验中加入扰动Δθ，调整控制器的比例增益Kp使得闭环系统在扰动下做临界等幅周期振荡，此时的比例增益称为临界增益Ku，此时的振荡周期称为临界振荡周期Tu：
例如，当Δθ＝30时，Ku＝4.9，Tu＝0.6，采集到角度AD值的临界振荡如图3所示；选取t＝[6，7区间，求得临界振荡周期Tu＝0.6，则PID控制器的整定参数为：
在本实施例中，农机运行时，通过姿态解算和卡尔曼最优估计进行车轮偏转角非接触式测量包括：农机运行时，车辆轮偏转角测量模块初始化，每隔预设时间(例如0.01s)采样陀螺仪A4的三轴角速度数据ωX、ωY、ωZ；设置陀螺仪A4的航向角角速度阈值
当ωZ大于角速度阈值时主控芯片对此时采样的角速度进行姿态解算，即陀螺仪A4自身所在坐标系为OX′Y′Z′，同时以右前轮轮胎中心为原点O,X轴平行于载体横轴向右，Y轴平行于载体纵轴指向前，Z轴平行于竖轴指向上，建立基础坐标系OXYZ，为了实时更新农机前轮的转向角度信息,需以车辆基础坐标系为参考,进行姿态更新，将陀螺仪所在载体坐标系OX′Y′Z′读取的角速度转换成以OXYZ为坐标系的角度数据；OXYZ坐标系经过Z→Y→X的旋转顺序得到OX′Y′Z′坐标系，分别利用姿态角变换和四元数表示从载体坐标系到基础坐标系的转换矩阵，通过反解即可求出传感器A相对于农机基础坐标系OXYZ的航向角ψA：
其中，四元数q0,q1,q2,q3在零时刻为0,ωx,ωy,ωz为陀螺仪A4向主控芯片输出的三轴角速度；通过陀螺仪不断输出的角速度对四元数进行迭代更新。
在本实施例中，若车轮转动速率
不为零时，主控芯片通过姿态传感器B3获取y轴方向上的加速度ay和车身转动的航向角ψB，采用阿卡曼转向模型，预估农机车轮转向角/>
式中：L是农机轴距；Vk是农机k时刻行驶速度；ay是姿态传感器B3输出的y轴方向上的加速度。
在本实施例中，根据
与/>
做差获得农机右前轮相对于车身的转向角速度/>
陀螺仪的累计误差为βk，对右前轮偏转角度/>
与βk的差做积分，得到右前轮转向角度估计值/>
为对车身和车轮的航向角求导数，即角速度，两者做差为相对角速度；
式中，dt是陀螺仪采样时间周期；k表示k时刻；βk表示陀螺仪的误差；陀螺仪的测量值存在误差，随之积分时间的增加而增加。
在本实施例中，主控芯片以右前轮转向角度估计值为角度测量系统状态量，以农机车轮转向角
为角度测量系统观测量，通过卡尔曼滤波器单元对车轮偏转角θfinal进行最优估计。
设该测量装置在0.01s内产生的陀螺仪角速率误差可认为为常量，即βk＝βk-1，结合步骤二积分公式得状态矩阵方程：
取
得系统状态方程：/>
k时刻角度测量系统观测量，得角度期望为：
同时建立观测方程为：
其中，Hk为观测矩阵；Vk为观测噪声，Vk～(0,Rk)。姿态传感器B3的动态测量精度为0.1m/s2，取Rk＝0.1。
设卡尔曼滤波单元初始状态量
假设采样间隔为dt＝0.01s，则初始参数为：
系统向卡尔曼滤波单元输出右前轮转向角度估计值
和运动学方程估计的车轮转向角/>
值时，单元开始初始化并开始对车轮航向角进行最优估计，具体实施步骤如下：
一步预测方程为：
状态递推方程：xk＝xk,k-1+KK(Zk-HKxk,k-1)；
增益方程为：
均方误差更新矩阵：Pk＝(I-KkH)Pk,k-1；Pk,k-1＝AP(k-1)AT+Q；式中，Q为系统噪声协方差矩阵，Q＝E(x xT)。
等待0.01s采样时间间隔，返回第一步开始执行。对不同农机，本航向角测量装置的卡尔曼滤波单元迭代次数略有不同，视具体情况而定；通过条件前馈控制器消除了电动方向盘2的响应死区，提高了系统响应速度。接着，通过车轮偏转角测量模块，消除了转向轮上陀螺仪在安装及农机行进时，敏感轴不垂直于地面从而产生的转向角度测量误差，并通过卡尔曼滤波单元对农机车轮航向角θ直接进行最优估计，消除了陀螺仪随时间误差的累积。
在本实施例中，所述根据实际转向角度实时计算转角偏差，以调整电动方向盘2转动角度包括：主控制器获取目标转角，实时计算转角偏差，通过前馈-PID控制算法，实时调整车轮目标转动角度值uk；
Δθk＝Δθk-1-θfinal,k；
其中，k为采样序列号，k＝0,1,2,…；uk为第k次采样时刻主控芯片输出的车轮转动角度值；Δθk为第k次采样时刻主控芯片输入的车轮目标转动角度值；Δθk-1为第k次采样时刻主控芯片输入的车轮目标转动角度值。通过减少对农机机械和液压油路的改装和传感器定位定向安装方法，普遍适用安装在不同的农用机械上，可以实现手动/自动的完全兼容。其次，通过条件前馈控制器消除了电动方向盘2的响应死区，提高了系统响应速度。接着，通过车轮偏转角测量模块，消除了转向轮上陀螺仪在安装及农机行进时，敏感轴不垂直于地面从而产生的转向角度测量误差，并通过卡尔曼滤波单元对农机车轮航向角直接进行最优估计，消除了陀螺仪随时间误差的累积。最后，通过PID控制对车轮偏转角实时校正，确保了转向的精度，满足了农机在自动驾驶时对车轮转向的要求。
实施例2
在实施例1的基础上，本实施例2还提供一种采用实施例1中基于stm32的轮式农机自动转向方法的转向系统，包括：初始化模块，初次安装或维修调试时，农机静置，运行初始化程序；测量模块，通过姿态解算和卡尔曼最优估计进行车轮偏转角的非接触式测量；调整模块，根据实际转向角度实时计算转角偏差，以调整电动方向盘转动角度；各模块的具体功能在实施例1中已经详细描述，不再赘述。
实施例3
在实施例1的基础上，本实施例3还提供一种采用实施例1中基于stm32的轮式农机自动转向方法的农机，包括：主控制器、姿态传感器、陀螺仪和电动方向盘2；所述电动方向盘2安装在农机车身转向立轴上；所述姿态传感器安装在农机车身钢架上；所述陀螺仪安装在车轮上任意随车轮转动的位置；所述主控制器适于采用如权利要求1所述的基于stm32的轮式农机自动转向方法对电动方向盘2进行控制。
综上所述，本发明通过控制柜1、电动方向盘2、陀螺仪和姿态传感器构成，stm32主控芯片工控板置于控制柜1中，电动方向盘2代替原有方向盘安装于转向立轴上，姿态传感器安装在车身钢架上任意位置，陀螺仪安装在车轮上任意随车轮转动的位置。该系统运行包含以下步骤：农机静止时，通过初始化程序测量转向系统响应死区，通过临界比例度法整定PID参数；农机作业时，转向控制系统在获取目标车轮转角后，通过姿态解算和卡尔曼滤波对车轮偏转角进行非接触测量，通过前馈-PID控制算法控制电动方向盘2转动，实现对农机转向系统的闭环控制。本发明实现了车轮转角传感器的任意位置安装，降低了转向装置调试门槛，增强了对不同农机的适应性，在农机行进过程中最大程度消除轮式农机转向误差。
在本申请所提供的几个实施例中，应该理解到，所揭露的装置和方法，也可以通过其它的方式实现。以上所描述的装置实施例仅仅是示意性的，例如，附图中的流程图和框图显示了根据本发明的多个实施例的装置、方法和计算机程序产品的可能实现的体系架构、功能和操作。在这点上，流程图或框图中的每个方框可以代表一个模块、程序段或代码的一部分，所述模块、程序段或代码的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令。也应当注意，在有些作为替换的实现方式中，方框中所标注的功能也可以以不同于附图中所标注的顺序发生。例如，两个连续的方框实际上可以基本并行地执行，它们有时也可以按相反的顺序执行，这依所涉及的功能而定。也要注意的是，框图和/或流程图中的每个方框、以及框图和/或流程图中的方框的组合，可以用执行规定的功能或动作的专用的基于硬件的系统来实现，或者可以用专用硬件与计算机指令的组合来实现。
另外，在本发明各个实施例中的各功能模块可以集成在一起形成一个独立的部分，也可以是各个模块单独存在，也可以两个或两个以上模块集成形成一个独立的部分。
所述功能如果以软件功能模块的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本发明各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。
以上述依据本发明的理想实施例为启示，通过上述的说明内容，相关工作人员完全可以在不偏离本项发明技术思想的范围内，进行多样的变更以及修改。本项发明的技术性范围并不局限于说明书上的内容，必须要根据权利要求范围来确定其技术性范围。
Claims (10)
1.一种基于stm32的轮式农机自动转向方法，其特征在于，包括：
初次安装或维修调试时，农机静置，运行初始化程序；
通过姿态解算和卡尔曼最优估计进行车轮偏转角非接触式测量；
获取目标转角，计算转角偏差，控制电动方向盘转动。
2.如权利要求1所述的基于stm32的轮式农机自动转向方法，其特征在于，
所述初次安装或维修调试时，农机静置，运行初始化程序包括：
运行初始化程序，以转向轮直行中位为输入零点，输入电动方向盘的旋转角度，输出通过农机偏转角测量模块采集转向轮的旋转角度，以测量横轴零点两侧的响应死区。
3.如权利要求2所述的基于stm32的轮式农机自动转向方法，其特征在于，
通过临界比例度法整定PID参数KP,Ki,Kd。
4.如权利要求3所述的基于stm32的轮式农机自动转向方法，其特征在于，
所述通过传感器姿态解算和卡尔曼最优估计进行车轮偏转角的非接触式测量包括：
农机运行时，农机车轮偏转角测量模块初始化，每隔预设时间采样陀螺仪A的三轴角速度数据ωX、ωY、ωZ；
设置陀螺仪A的航向角角速度阈值，当ωZ大于角速度阈值时主控制器对此时采样的角速度进行姿态解算。
分别利用姿态角变换和四元数表示从载体坐标系到基础坐标系的转换矩阵，通过反解求出传感器A相对于农机基础坐标系OXYZ的航向角ψA。
5.如权利要求4所述的基于stm32的轮式农机自动转向方法，其特征在于，
若车轮转动速率
不为零时，主控制器通过姿态传感器B获取y轴方向上的加速度ay和车身转动的航向角ψB，采用阿卡曼转向模型，预估农机车轮转向角/>
6.如权利要求5所述的基于stm32的轮式农机自动转向方法，其特征在于，
根据
与/>
获得农机右前轮相对于车身的转向角速度/>
陀螺仪的累计误差为βk，对右前轮偏转角度
与βk的差做积分，得到右前轮转向角度估计值/>
7.如权利要求6所述的基于stm32的轮式农机自动转向方法，其特征在于，
主控制器以右前轮转向角度估计值
为角度测量系统状态量，以农机车轮转向角/>
为角度测量系统观测量，通过卡尔曼滤波器单元对车轮偏转角θfinal进行最优估计。
8.如权利要求7所述的基于stm32的轮式农机自动转向方法，其特征在于，
所述根据实际转向角度实时计算转角偏差，以调整电动方向盘转动角度包括：
把实际转向角度θfinal传递给主控制器，计算转角偏差，通过前馈-PID控制算法，实时调整车轮目标转动角度值uk。
9.一种采用如权利要求1所述基于stm32的轮式农机自动转向方法的转向系统，其特征在于，包括：
初始化模块，初次安装或维修调试时，农机静置，运行初始化程序；
测量模块，通过姿态解算和卡尔曼最优估计进行车轮偏转角的非接触式测量；
调整模块，根据实际转向角度实时计算转角偏差，以调整电动方向盘转动角度。
10.一种采用如权利要求1所述基于stm32的轮式农机自动转向方法的农机，其特征在于，包括：
主控制器、姿态传感器、陀螺仪和电动方向盘；
所述电动方向盘安装在农机车身转向立轴上；
所述姿态传感器安装在农机车身钢架上；
所述陀螺仪安装在车轮上任意随车轮转动的位置；
所述主控制器适于采用如权利要求1所述的基于stm32的轮式农机自动转向方法对电动方向盘进行控制。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310493685.7A CN116395027A (zh) | 2023-04-26 | 2023-04-26 | 一种基于stm32的轮式农机自动转向方法、系统及农机 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310493685.7A CN116395027A (zh) | 2023-04-26 | 2023-04-26 | 一种基于stm32的轮式农机自动转向方法、系统及农机 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN116395027A true CN116395027A (zh) | 2023-07-07 |
Family
ID=87018006
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310493685.7A Pending CN116395027A (zh) | 2023-04-26 | 2023-04-26 | 一种基于stm32的轮式农机自动转向方法、系统及农机 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116395027A (zh) |
-
2023
- 2023-04-26 CN CN202310493685.7A patent/CN116395027A/zh active Pending
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN107415939B (zh) | 一种分布式驱动电动汽车转向稳定性控制方法 | |
| EP3171134B1 (en) | Sensor alignment calibration | |
| US5579228A (en) | Steering control system for trailers | |
| US7477973B2 (en) | Vehicle gyro based steering assembly angle and angular rate sensor | |
| CN107607113B (zh) | 一种两轴姿态倾角测量方法 | |
| US8352132B2 (en) | Automatic blade control system with integrated global navigation satellite system and inertial sensors | |
| CN111873991B (zh) | 一种车辆转向的控制方法、装置、终端及存储介质 | |
| AU2016256796A1 (en) | Single-mode implement steering | |
| Marco et al. | Multi-modal sensor fusion for highly accurate vehicle motion state estimation | |
| CN108438048B (zh) | 一种新型的履带式拖拉机自动转向控制系统及控制方法 | |
| CN112965373B (zh) | 一种农用和矿用铰接式车辆路径跟踪控制方法 | |
| CN110244718B (zh) | 一种可自动避障的巡视智能小车 | |
| CN110806754A (zh) | 四旋翼飞行器姿态修正控制系统及方法 | |
| CN108507572A (zh) | 一种基于mems惯性测量单元的姿态定位误差修正方法 | |
| KR20180095225A (ko) | 차량 및 차량의 제어 방법 | |
| CN114413791B (zh) | 一种舵轮式叉车舵轮零偏角的检测方法、系统及装置 | |
| CN108871323A (zh) | 一种低成本惯性传感器在机动环境下的高精度导航方法 | |
| CN111679681B (zh) | 一种基于滑模鲁棒控制的水下机器人运动路径跟踪方法 | |
| CN116395027A (zh) | 一种基于stm32的轮式农机自动转向方法、系统及农机 | |
| CN112026749B (zh) | 一种线控转向系统稳定性控制方法 | |
| CN111189443A (zh) | 一种在线校准步长、修正运动偏差角和自适应能量管理的行人导航方法 | |
| CN108413923B (zh) | 一种基于鲁棒混合滤波的车辆侧倾角与俯仰角估计方法 | |
| US11801893B2 (en) | Steering controller and method for steering a tracking path of a trailer | |
| CN114815583A (zh) | 一种车载式草莓采摘机器人稳像方法 | |
| CN113188565B (zh) | 一种机载分布式pos传递对准量测异常处理方法 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| TA01 | Transfer of patent application right |
Effective date of registration: 20231204 Address after: No. 301 Xuefu Road, Zhenjiang City, Jiangsu Province, 212000 Applicant after: JIANGSU University Applicant after: Shanghai Shangcheng Modern Agricultural Development Co.,Ltd. Address before: No. 301 Xuefu Road, Zhenjiang City, Jiangsu Province, 212000 Applicant before: JIANGSU University |
|
| TA01 | Transfer of patent application right |