CN107509443B - 一种智能割草机的坡地行驶控制方法及系统 - Google Patents
一种智能割草机的坡地行驶控制方法及系统 Download PDFInfo
- Publication number
- CN107509443B CN107509443B CN201710790701.3A CN201710790701A CN107509443B CN 107509443 B CN107509443 B CN 107509443B CN 201710790701 A CN201710790701 A CN 201710790701A CN 107509443 B CN107509443 B CN 107509443B
- Authority
- CN
- China
- Prior art keywords
- landslide
- acceleration
- driving
- intelligent mower
- submodule
- 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.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 61
- 230000001133 acceleration Effects 0.000 claims abstract description 263
- 238000006073 displacement reaction Methods 0.000 claims description 110
- 238000012545 processing Methods 0.000 claims description 50
- 238000005070 sampling Methods 0.000 claims description 17
- 238000001514 detection method Methods 0.000 claims description 12
- 238000004364 calculation method Methods 0.000 claims description 8
- 238000009825 accumulation Methods 0.000 claims description 3
- 230000004044 response Effects 0.000 abstract description 4
- 230000008569 process Effects 0.000 description 33
- 238000010586 diagram Methods 0.000 description 12
- 230000008859 change Effects 0.000 description 6
- 210000001331 nose Anatomy 0.000 description 5
- 244000025254 Cannabis sativa Species 0.000 description 4
- 238000005516 engineering process Methods 0.000 description 3
- 210000003128 head Anatomy 0.000 description 3
- 101100046503 Symbiobacterium thermophilum (strain T / IAM 14863) tnaA1 gene Proteins 0.000 description 2
- 238000004458 analytical method Methods 0.000 description 2
- 238000004422 calculation algorithm Methods 0.000 description 2
- 238000005259 measurement Methods 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000004088 simulation Methods 0.000 description 2
- 238000006467 substitution reaction Methods 0.000 description 2
- NAWXUBYGYWOOIX-SFHVURJKSA-N (2s)-2-[[4-[2-(2,4-diaminoquinazolin-6-yl)ethyl]benzoyl]amino]-4-methylidenepentanedioic acid Chemical compound C1=CC2=NC(N)=NC(N)=C2C=C1CCC1=CC=C(C(=O)N[C@@H](CC(=C)C(O)=O)C(O)=O)C=C1 NAWXUBYGYWOOIX-SFHVURJKSA-N 0.000 description 1
- 206010063385 Intellectualisation Diseases 0.000 description 1
- 238000013459 approach Methods 0.000 description 1
- 230000001174 ascending effect Effects 0.000 description 1
- 238000006243 chemical reaction Methods 0.000 description 1
- 230000007423 decrease Effects 0.000 description 1
- 238000012938 design process Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000005484 gravity Effects 0.000 description 1
- 230000010354 integration Effects 0.000 description 1
- 238000012423 maintenance Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
-
- A—HUMAN NECESSITIES
- A01—AGRICULTURE; FORESTRY; ANIMAL HUSBANDRY; HUNTING; TRAPPING; FISHING
- A01D—HARVESTING; MOWING
- A01D34/00—Mowers; Mowing apparatus of harvesters
- A01D34/006—Control or measuring arrangements
- A01D34/008—Control or measuring arrangements for automated or remotely controlled operation
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0219—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory ensuring the processing of the whole working surface
Landscapes
- Engineering & Computer Science (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Life Sciences & Earth Sciences (AREA)
- Environmental Sciences (AREA)
- Harvester Elements (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明涉及割草机技术领域，具体公开了一种智能割草机的坡地行驶控制方法及系统，在智能割草机发生滑坡偏离原始航线后，能够实时规划出最优的行驶路线，使得偏离的智能割草机能够最优地驶回原始航线，同时避免出现智能割草机因坡地因素在多坡地段重复路线割草的问题，极大地提高了割草的覆盖率，基本实现无割草盲区。本发明提供的一种智能割草机的坡地行驶控制系统，设置的高可靠性的三轴加速度传感器、三轴角速度传感器及高精度的MCU控制模块使得智能割草机反应更灵敏、割草更精准、智能化程度更高，能够极大地提高智能割草机对复杂工作环境的适应性，拓宽其适用范围。
Description
技术领域
本发明涉及割草机技术领域，尤其涉及一种智能割草机的坡地行驶控制方法及系统。
背景技术
随着社会的智能化快速发展，人们对草坪的自动化维护需求日益提高，手工割草机已经不能满足高效化、强可控性的时代要求，在这种形势下，智能割草机在不断被改进。现今智能割草机大多数为后轮采用电气控制、前轮采用万向轮的支撑结构，在行驶过程中的前进、后退、转弯等动作主要凭靠智能割草机后轮的左右电机，这种智能割草机主要应用在平整、无细小障碍物的草坪执行直线割草任务。所述左右电机采用的PWM(脉冲宽度调制，单位时间内脉冲宽度等效于与脉宽参数有关的电压)技术是一种常见的电机转速控制技术，在智能割草机行驶方面用于控制左右轮电机的转速。
沿直线割草是一种普遍、高效的割草方式，在平地上基本可以做到全覆盖、无割草盲区，但是在坡地、丘陵等地势复杂的地面上，智能割草机由于后轮驱动、前轮万向轮偏转的关系，在遇到草坡时出现滑坡，导致自身偏离原始航线，进入新的航线而无法回到所述原始航线，从而出现割草盲区。所以，要求智能割草机在滑坡偏离原始航线后能及时地自动驶回原始航线，其中，如何行驶最优路线是问题的关键。
在最优路线的设计过程中，需要对智能割草机的实时运动数据做出全方位的掌控，便于智能割草机的MCU控制模块自动地检测到自身此时的运动状态，从而根据智能割草机的实时行驶情况(其中包括实际坡地环境)执行最优算法而设计出驶回原始航线的最优路线。
发明内容
本发明提供一种智能割草机的坡地行驶控制技术方案，所解决的技术问题是，在智能割草机在行驶过程中发生滑坡时，使其沿最优过草坡行驶路线驶回原始航线。
为解决以上技术问题，本发明提供一种智能割草机的坡地行驶控制方法，包括以下步骤：
S1.根据预设的初始航向角控制所述智能割草机在原始航线上匀速直线行驶；
S2.检测是否存在滑坡行驶的标志位；若否，则处理当前采集的三轴角速度和三轴加速度，并在判定所述智能割草机结束滑坡时生成所述滑坡行驶的标志位和计算得到滑坡位移；若是，则执行步骤S3；
S3.根据计算获得的与所述滑坡行驶的标志位对应的滑坡方向，将所述智能割草机的行驶方向调整至与所述原始航线垂直并实时记录采集获得的所述三轴角速度和所述三轴加速度；
S4.根据当前记录的所述三轴加速度和所述三轴角速度，计算得到所述智能割草机在所述滑坡方向上的实时调整位移；
S5.根据实时记录的所述三轴角速度和所述三轴加速度，实时调整所述智能割草机的行驶方向和行驶速度，直到所述实时调整位移为零值时，对所述滑坡位移和所述滑坡行驶的标志位清零，并返回到所述步骤S1。
进一步地，在所述步骤S2中，处理当前采集的三轴角速度和三轴加速度，并在判定所述智能割草机结束滑坡时生成所述滑坡行驶的标志位和计算得到滑坡位移，具体包括：
S2-1.根据实时采集的所述三轴加速度在所述滑坡方向上的滑坡加速度，判断所述智能割草机是否在滑坡行驶；若是，则执行步骤S2-2；若否，则执行步骤S2-3；
S2-2.记录滑坡开始时刻，根据当前记录的三轴角速度和三轴加速度计算并保存所述智能割草机从所述滑坡开始时刻起在所述滑坡方向上的滑坡加速度，并返回到所述步骤S1；
S2-3.检测是否存在所述智能割草机在水平面上的水平滑坡加速度；若否，则返回到所述步骤S1；若是，则执行步骤S2-4；
S2-4.记录滑坡结束时刻，计算得到滑坡时间，并根据所述滑坡时间和在所述滑坡时间内的所述水平滑坡加速度，计算并记录平均滑坡加速度和滑坡位移；
S2-5.清零所述水平滑坡加速度，生成所述滑坡行驶的标志位。
更进一步地，所述步骤S5具体包括：
S5-1.根据预设的时间间隔，对每一时刻的所述三轴加速度中的所述滑坡加速度、所述原始航线所在方向上的航向加速度和所述三轴角速度中的横滚角进行实时采样，计算得到各个时间间隔内的位移调整变化量，并进一步计算得到当前时刻的平均调整航向加速度、平均目标航向加速度；
S5-2.判断所述滑坡位移与当前各个时间间隔内累计获得的所述实时调整位移是否大小相同；若是，则对所述滑坡位移和所述滑坡行驶的标志位清零，并返回到所述步骤S1；若否，则执行步骤S5-3；
S5-3.判断所述平均调整航向加速度与所述平均目标航向加速度是否相同；若是，则通过PWM信号控制所述智能割草机的左后轮转速大于右后轮转速；若否，则通过PWM信号控制所述智能割草机的左后轮转速小于右后轮转速；
S5-4.返回到所述步骤S5-1。
本发明还提供一种智能割草机的坡地行驶控制系统，包括MCU控制模块及与其连接的三轴加速度传感器和三轴角速度传感器；
所述三轴加速度传感器和三轴角速度传感器分别用于实时采集所述智能割草机的三轴加速度和三轴角速度，并将所述三轴加速度和所述三轴角速度发送到所述MCU控制模块；
所述MCU控制模块设有平稳行驶驱动子模块、标志位检测与处理子模块、滑坡调整驱动子模块；
所述平稳行驶驱动子模块，用于根据预设的初始航向角控制所述智能割草机在原始航线上匀速直线行驶；
所述标志位检测与处理子模块，用于在未检测到有滑坡行驶的标志位时，处理所述三轴加速度传感器当前采集的三轴加速度和所述三轴角速度传感器当前采集的三轴角速度，并在判定所述智能割草机结束滑坡时，生成所述滑坡行驶的标志位和计算得到滑坡位移；
所述标志位检测与处理子模块，还用于在检测到存在滑坡行驶的标志位时，计算获得与所述滑坡行驶的标志位对应的滑坡方向；
所述滑坡调整驱动子模块，用于根据所述滑坡方向将所述智能割草机的行驶方向调整至与所述原始航线垂直，并实时记录采集获得的所述三轴角速度和所述三轴加速度；
所述滑坡调整驱动子模块，还用于处理当前记录的所述三轴加速度和所述三轴角速度，计算得到所述智能割草机在所述滑坡方向上的实时调整位移；
所述滑坡调整驱动子模块，还用于根据实时记录的所述三轴角速度和所述三轴加速度，实时调整所述智能割草机的行驶方向和行驶速度，直到所述实时调整位移为零值时，对所述滑坡位移和所述滑坡行驶的标志位清零。
具体地，所述三轴加速度包含所述智能割草机在所述原始航线所在方向上的航向加速度、在所述滑坡方向上的滑坡加速度；所述三轴角速度包含航向角速度、横滚角速度，所述航向角速度、横滚角速度转化为航向角和横滚角，所述航向角包含所述初始航向角。
具体地，所述标志位检测与处理子模块设有滑坡行驶判定与处理子模块、数据记录与清零子模块，以及，标志位生成子模块；
所述滑坡行驶判定与处理子模块，用于在判定所述智能割草机在滑坡行驶时，根据实时采集的所述三轴加速度在所述滑坡方向上的滑坡加速度，先后驱动所述数据记录与清零子模块、所述平稳行驶驱动子模块工作；在判定所述智能割草机未在滑坡行驶、并检测到所述智能割草机在水平面上存在水平滑坡加速度时，驱动所述数据记录与清零子模块工作；在判定所述智能割草机未在滑坡行驶、并检测到未有记录的所述智能割草机在水平面上的水平滑坡加速度时，驱动所述平稳行驶驱动子模块工作；
所述数据记录与清零子模块，用于响应所述滑坡行驶判定与处理子模块的驱动，记录滑坡开始时刻，根据当前记录的三轴角速度和三轴加速度计算并保存所述智能割草机从所述滑坡开始时刻起在所述滑坡方向上的滑坡加速度，并驱动所述平稳行驶驱动子模块工作；记录滑坡结束时刻，计算得到滑坡时间，并根据所述滑坡时间和在所述滑坡时间内的所述水平滑坡加速度，计算并记录得到平均滑坡加速度和滑坡位移，清零所述水平滑坡加速度，驱动所述标志位生成子模块工作；
所述平稳行驶驱动子模块，用于响应所述滑坡行驶判定与处理子模块及所述数据记录与清零子模块的驱动，根据所述预设的初始航向角控制所述智能割草机在所述原始航线上匀速直线行驶；
所述标志位生成子模块，用于响应数据记录与清零子模块的驱动，生成所述滑坡行驶的标志位。
更具体地，所述滑坡调整驱动子模块设有加速度计算子模块、判断与处理子模块和车轮驱动子模块；
所述加速度计算子模块，用于根据预设的时间间隔，对每一时刻的所述三轴加速度中的所述滑坡加速度、所述航向加速度和所述三轴角速度中的横滚角进行实时采样，计算得到各个时间间隔内的位移调整变化量，并进一步计算得到当前时刻的平均调整航向加速度、平均目标航向加速度；
所述判断与处理子模块，用于判定所述滑坡位移与当前各个时间间隔内的位移调整变化量累计的所述实时调整位移之间的差值大于0时，驱动所述数据记录与清零子模块工作；
所述数据记录与清零子模块，还用于响应所述判断与处理子模块的驱动，对所述滑坡位移和所述滑坡行驶的标志位清零；
所述判断与处理子模块，还用于在判定所述滑坡位移与当前各个时间间隔内累计的所述实时调整位移之间的差值小于或等于0，并且所述平均调整航向加速度与所述平均目标航向加速度之间的差值大于0时，驱动所述车轮驱动子模块工作；或者，在判定所述平均调整航向加速度与平均目标航向加速度之间的差值小于或等于0时，驱动所述车轮驱动子模块工作；
所述车轮驱动子模块，用于响应所述判断与处理子模块的驱动，通过PWM信号控制所述智能割草机的左后轮转速大于右后轮转速；或者，通过PWM信号控制所述智能割草机的左后轮转速小于右后轮转速。
本发明提供的一种智能割草机的坡地行驶控制系统，还包括独立连接所述车轮驱动子模块的左后轮行驶电机和右后轮行驶电机；所述左后轮行驶电机，用于接收所述车轮驱动子模块发出的PWM信号，独立调整所述智能割草机的左后轮转速；所述右后轮行驶电机，用于接收所述车轮驱动子模块发出的PWM信号，独立调整所述智能割草机的右后轮转速。
具体地，所述智能割草机的左前轮和右前轮为采用支撑结构的万向轮。
本发明提供的一种智能割草机的坡地行驶控制方法与系统，在智能割草机发生滑坡偏离原始航线后，能够实时规划出最优的行驶路线，使得偏离的智能割草机能够最优地驶回原始航线，同时避免出现智能割草机因坡地因素在多坡地段重复路线割草的问题，极大地提高了割草的覆盖率，基本实现无割草盲区。本发明提供的一种智能割草机的坡地行驶控制系统，设置的高可靠性的三轴加速度传感器、三轴角速度传感器及高精度的MCU控制模块使得智能割草机反应更灵敏、割草更精准、智能化程度更高，能够极大地提高智能割草机对复杂工作环境的适应性，拓宽其适用范围。
附图说明
图1是本发明提供的一种智能割草机的坡地行驶控制方法的一个实施例的步骤流程图；
图2是本发明提供的图1实施例中的步骤S2的一种实现方式的步骤流程图；
图3是本发明提供的图1实施例中的步骤S5的一种实现方式的步骤流程图；
图4是本发明提供的一种智能割草机的坡地行驶控制方法又一个实施例的步骤流程图；
图5是本发明提供的一种智能割草机的坡地行驶控制系统的一个实施例的结构示意图；
图6是本发明提供的一种智能割草机的坡地行驶控制技术方案的坡地行驶路线模拟图；
图7-1是本发明提供的一种智能割草机的坡地行驶控制系统的智能割草机处于草坡上的参考坐标系的正面视角图；
图7-2是本发明提供的一种智能割草机的坡地行驶控制系统的智能割草机处于草坡上的参考坐标系的侧面视角图；
图8是本发明提供的图6实施例中智能割草机在步骤S3～S5过程中的a方向与s方向的区别示意图；
图9是本发明提供的图6实施例中智能割草机在步骤S2过程中的水平方向加速度与时间的关系图；
图10是本发明提供的图6实施例中智能割草机在步骤S2过程中的平均滑坡加速度与时间的关系等效图；
图11是本发明提供的图6实施例中智能割草机在步骤S3过程中参考坐标系的侧面视角图；
图12是本发明提供的图6实施例中智能割草机在步骤S3～S5过程中的最优曲线的行驶物理模型图；
图13是本发明提供的图9实施例中的智能割草机在步骤S3～S5过程中的行驶方向示意图；
图14是本发明实施例提供的图4实施例的算法程序图。
具体实施方式
下面结合附图具体阐明本发明的实施方式，实施例的给出仅仅是为了说明目的，并不能理解为对本发明的限定，包括附图仅供参考和说明使用，不构成对本发明专利保护范围的限制，因为在不脱离本发明精神和范围基础上，可以对本发明进行许多改变。
图1是本发明实施例提供的一种智能割草机的坡地行驶控制方法的一个实施例的步骤流程图。本实施例提供的智能割草机的坡地行驶控制方法，包括以下步骤：
S1.根据预设的初始航向角α控制所述智能割草机M在原始航线L上匀速直线行驶；
S2.检测是否存在滑坡行驶的标志位(“坡地”)；若否(即，“坡地＝0”)，则处理当前采集的三轴角速度和三轴加速度，并在判定所述智能割草机M结束滑坡时生成所述滑坡行驶的标志位(即，“坡地＝1”)和计算得到滑坡位移S(Hor)；若是，则执行步骤S3；
S3.根据计算获得的与所述滑坡行驶的标志位(“坡地＝1”)对应的滑坡方向，将所述智能割草机M的行驶方向调整至与所述原始航线L垂直并实时记录采集获得的所述三轴角速度和所述三轴加速度；
S4.处理当前记录的所述三轴加速度和所述三轴角速度，计算得到所述智能割草机M在所述滑坡方向上的实时调整位移S0"(Hor)；
S5.根据实时记录的所述三轴角速度和所述三轴加速度，实时调整所述智能割草机M的行驶方向和行驶速度，直到所述实时调整位移S0"(Hor)为零值时，对所述滑坡位移S(Hor)和所述滑坡行驶的标志位(“坡地＝1”)清零，并返回到所述步骤S1。
在图1中，所述滑坡行驶的标志位即“坡地＝1”，若不存在滑坡行驶的标志位或对所述滑坡行驶的标志位(“坡地＝1”)清零，即“坡地＝0”，在所述步骤S1及S1之前，所述智能割草机M都是处于“坡地＝0”的状态。
参见图2，是本发明提供的图1实施例中所述步骤S2的一种实现方式的步骤流程图。在所述步骤S2中，处理当前采集的三轴角速度和三轴加速度，并在判定所述智能割草机M结束滑坡时生成所述滑坡行驶的标志位(“坡地＝1”)和计算得到滑坡位移S(Hor)，具体包括：
S2-1.根据实时采集的所述三轴加速度在所述滑坡方向上的滑坡加速度a(X)，判断所述智能割草机M是否在滑坡行驶；若是，则执行步骤S2-2；若否，则执行步骤S2-3；
S2-2.记录滑坡开始时刻t1，根据当前记录的三轴角速度和三轴加速度计算并保存所述智能割草机M从所述滑坡开始时刻t1起在所述滑坡方向上的滑坡加速度，并返回到所述步骤S1；
S2-3.检测是否存在所述智能割草机M在水平面上的水平滑坡加速度a(Hor)；若否，则返回到所述步骤S1；若是，则执行步骤S2-4；
S2-4.记录滑坡结束时刻t2，计算得到滑坡时间，并根据所述滑坡时间和在所述滑坡时间内的所述水平滑坡加速度a(Hor)，计算并记录平均滑坡加速度a(avg)和滑坡位移S(Hor)；
S2-5.清零所述水平滑坡加速度a(Hor)，生成所述滑坡行驶的标志位(“坡地＝1”)。
参见，图3是本发明提供的图1实施例中的步骤S5的一种实现方式的步骤流程图。在图3中，所述步骤S5具体包括：
S5-1.根据预设的时间间隔，对每一时刻的所述三轴加速度中的所述滑坡加速度a(X)、所述原始航线L方向上的航向加速度a(Y)和所述三轴角速度中的横滚角Roll进行实时采样，计算得到各个时间间隔内的位移调整变化量△S"(n)，并进一步计算得到当前时刻的平均调整航向加速度ans"(avg)、平均目标航向加速度ans"(avg)*；
S5-2.判断所述滑坡位移S(Hor)与当前各个时间间隔内累计获得的所述实时调整位移S0"(Hor)是否大小相同；若是，则对所述滑坡位移S(Hor)和所述滑坡行驶的标志位(“坡地＝1”)清零(即“坡地＝0”)，并返回到所述步骤S1；若否，则执行步骤S5-3；
S5-3.判断所述平均调整航向加速度ans"(avg)与所述平均目标航向加速度ans"(avg)*是否相同；若是，则通过PWM信号控制所述智能割草机M的左后轮转速大于右后轮转速；若否，则通过PWM信号控制所述智能割草机M的左后轮转速小于右后轮转速；
S5-4.返回到所述步骤S5-1。
参见图4，是本发明提供的智能割草机M的坡地行驶控制方法的又一个实施例的步骤流程图，其中包含图1～图3的完整过程，与图1～图3形成简化的对应关系。
与本发明实施例提供的一种智能割草机M的坡地行驶控制方法对应的是，本发明还提供一种智能割草机的坡地行驶控制系统。参见图5，本发明提供的智能割草机M的坡地行驶控制系统的一个实施例的结构示意图。
具体地，本发明实施例提供的一种智能割草机M的坡地行驶控制系统主要包括：MCU控制模块10及与其连接的三轴加速度传感器20和三轴角速度传感器30；
所述三轴加速度传感器20和三轴角速度传感器30分别用于实时采集所述智能割草机M的三轴加速度和三轴角速度，并将所述三轴加速度和所述三轴角速度发送到所述MCU控制模块10；
所述MCU控制模块10设有平稳行驶驱动子模块11、标志位检测与处理子模块12、滑坡调整驱动子模块13；
所述平稳行驶驱动子模块11，用于根据预设的初始航向角α控制所述智能割草机M在原始航线L上匀速直线行驶；
所述标志位检测与处理子模块12，还用于在未检测到有滑坡行驶的标志位时(“坡地＝0”)，处理所述三轴加速度传感器20当前采集的三轴加速度和所述三轴角速度传感器30当前采集的三轴角速度，并在判定所述智能割草机M结束滑坡时，生成所述滑坡行驶的标志位(“坡地＝1”)和计算得到滑坡位移S(Hor)；
所述标志位检测与处理子模块12，还用于在检测到存在滑坡行驶的标志位(“坡地＝1”)时，计算获得与所述滑坡行驶的标志位(“坡地＝1”)对应的滑坡方向；
所述滑坡调整驱动子模块13，用于根据所述滑坡方向将所述智能割草机M的行驶方向调整至与所述原始航线L垂直，并实时记录采集获得的所述三轴角速度和所述三轴加速度；
所述滑坡调整驱动子模块13，还用于处理当前记录的所述三轴加速度和所述三轴角速度，计算得到所述智能割草机M在所述滑坡方向上的实时调整位移S0"(Hor)；
所述滑坡调整驱动子模块13，还用于根据实时记录的所述三轴角速度和所述三轴加速度，实时调整所述智能割草机M的行驶方向和行驶速度，直到所述实时调整位移S0"(Hor)为零值时，对所述滑坡位移S(Hor)和所述滑坡行驶的标志位(“坡地＝1”)清零。
其中，所述滑坡行驶的标志位即“坡地＝1”，若不存在滑坡行驶的标志位或对所述滑坡行驶的标志位(“坡地＝1”)清零，即“坡地＝0”，在所述步骤S1及S1之前，所述智能割草机M都是处于“坡地＝0”的状态。
需要说明的是：
所述三轴加速度包含所述智能割草机M在所述原始航线L方向上的航向加速度a(Y)、在所述滑坡方向上的滑坡加速度a(X)；所述三轴角速度包含航向角速度、横滚角速度，所述航向角速度、横滚角速度转化为航向角Yaw和横滚角Roll，所述航向角Yaw包含所述初始航向角α。
所述标志位检测与处理子模块12设有滑坡行驶判定与处理子模块121、数据记录与清零子模块122，以及，标志位生成子模块123；
所述滑坡行驶判定与处理子模块121，用于在判定所述智能割草机M在滑坡行驶时，根据实时采集的所述三轴加速度在所述滑坡方向上的滑坡加速度a(X)，先后驱动所述数据记录与清零子模块122、所述平稳行驶驱动子模块11工作；在判定所述智能割草机M未在滑坡行驶、并检测到所述智能割草机M在水平面上存在水平滑坡加速度a(X)时，驱动所述数据记录与清零子模块122工作；在判定所述智能割草机M未在滑坡行驶、并检测到未有记录的所述智能割草机M在水平面上的水平滑坡加速度a(X)时，驱动所述平稳行驶驱动子模块11工作；
所述数据记录与清零子模块122，用于响应所述滑坡行驶判定与处理子模块121的驱动，记录滑坡开始时刻t1，根据当前记录的三轴角速度和三轴加速度计算并保存所述智能割草机M从所述滑坡开始时刻t1起在所述滑坡方向上的滑坡加速度a(X)，并驱动所述平稳行驶驱动子模块11工作；记录滑坡结束时刻t2，计算得到滑坡时间，并根据所述滑坡时间和在所述滑坡时间内的滑坡加速度a(X)，计算并记录得到平均滑坡加速度a(X)和滑坡位移S(Hor)，清零所述水平滑坡加速度a(X)，驱动所述标志位生成子模块123工作；
所述平稳行驶驱动子模块11，用于响应所述滑坡行驶判定与处理子模块121及所述数据记录与清零子模块122的驱动，根据所述预设的初始航向角α控制所述智能割草机M在所述原始航线L上匀速直线行驶；
所述标志位生成子模块123，用于响应数据记录与清零子模块122的驱动，生成所述滑坡行驶的标志位(“坡地＝1”)。
所述滑坡调整驱动子模块13设有加速度计算子模块131、判断与处理子模块132和车轮驱动子模块133；
所述加速度计算子模块131，用于根据预设的时间间隔，对每一时刻的所述三轴加速度中的所述滑坡加速度a(X)、所述原始航线L方向上的航向加速度a(Y)和所述三轴角速度中的横滚角Roll进行实时采样，计算得到各个时间间隔内的位移调整变化量△S"(n)，并进一步计算得到当前时刻的平均调整航向加速度ans"(avg)、平均目标航向加速度ans"(avg)*；
所述判断与处理子模块132，用于判定所述滑坡位移S(Hor)与当前各个时间间隔内的位移调整变化量△S"(n)累计的所述实时调整位移S0"(Hor)之间的差值大于0时，驱动所述数据记录与清零子模块122工作；
所述数据记录与清零子模块122，还用于响应所述判断与处理子模块132的驱动，对所述滑坡位移S(Hor)和所述滑坡行驶的标志位(“坡地＝1”)清零；
所述判断与处理子模块132，还用于在判定所述滑坡位移S(Hor)与当前各个时间间隔内累计的所述实时调整位移S0"(Hor)之间的差值小于或等于0，并且所述平均调整航向加速度ans"(avg)与所述平均目标航向加速度ans"(avg)*之间的差值大于0时，驱动所述车轮驱动子模块133工作；或者，在判定所述平均调整航向加速度ans"(avg)与平均目标航向加速度ans"(avg)*之间的差值小于或等于0时，驱动所述车轮驱动子模块133工作；
所述车轮驱动子模块133，用于响应所述判断与处理子模块132的驱动，通过PWM信号控制所述智能割草机M的左后轮(LR)转速大于右后轮(RR)转速；或者，通过PWM信号控制所述智能割草机M的左后轮转速小于右后轮转速。
本发明提供的一种智能割草机M的坡地行驶控制系统，还包括独立连接所述车轮驱动子模块133的左后轮行驶电机40和右后轮行驶电机50；所述左后轮行驶电机40，用于接收所述车轮驱动子模块133发出的PWM信号，独立调整所述智能割草机M的左后轮(LR)转速；所述右后轮行驶电机50，用于接收所述车轮驱动子模块133发出的PWM信号，独立调整所述智能割草机M的右后轮(RR)转速。所述智能割草机M的左前轮LF和右前轮RF为采用支撑结构的万向轮。
在智能割草机的控制领域中，其实时行驶情况通常由姿态传感器获取，它是基于MEMS(Micro-Electro-Mechanical System，微机电系统)技术的高性能三维运动姿态测量系统，它包含三轴陀螺仪、三轴加速度传感器、三轴电子罗盘等运动传感器，通过内嵌的低功耗ARM处理器得到经过温度补偿的三维姿态与方位等可靠数据。常用的姿态传感器包含三轴姿态传感器，其中单独设有三轴陀螺仪或三轴加速度传感器或三轴电子罗盘等；包含六轴姿态传感器，其中设有相互结合的三轴陀螺仪、三轴加速度传感器或三轴陀螺仪、三轴电子罗盘等；包含九轴姿态传感器，其中设有结合的三轴陀螺仪、三轴电子罗盘和三轴加速度传感器等。基于此，本实施例采用的是三轴加速度传感器20和三轴角速度传感器30结合的六轴姿态传感器23。
参见图6，是本发明实施例提供的一种智能割草机的坡地行驶控制技术方案的坡地行驶路线模拟图。以智能割草机M的坡地行驶控制方法为分析对象，所述智能割草机M的行驶路线A如图2所示，依序分为平稳上坡阶段：①——>②，对应于所述步骤S1；滑坡阶段：②——>③，对应于所述步骤S2，产生的滑坡位移S(Hor)即从②到③的位移；调整阶段：③——>④——>⑤，对应于所述步骤S3～S5；在所述平稳上坡之前存在一个平地直线阶段：①之前，对应于所述步骤S1；在所述调整阶段过后也存在一个平地直线阶段：⑤之后，对应于所述步骤S1。其中，①处代表智能割草机M上坡的起点；②处代表智能割草机M滑坡的开始点；③处代表智能割草机M滑坡的结束点，也即智能割草机M向所述原始航线L调整的起点；④处代表智能割草机M的行驶方向调整了45°的临界点，⑤处代表智能割草机M调整回所述原始航线L的临界点。
当所述智能割草机M出现滑坡时，若不及时地对其进行调整，则从③开始，它会沿着偏离路线B：③——>④"行驶而去，从而偏离了所述原始航线L：①——>②——>⑤。其中，④"处代表，当所述智能割草机M发生滑坡时，若不对其进行及时的调整，所述智能割草机M下坡的终点。
参见图7-1和7-2，分别是本发明实施例提供的一种智能割草机的坡地行驶控制技术方案的智能割草机处于草坡上的参考坐标系的正面视角图和侧面视角图。所述三轴加速度传感器20测得的所述三轴加速度包括三维直角坐标系下的所述航向加速度a(Y)、所述滑坡加速度a(X)(及垂直于航向加速度a(Y)、所述滑坡加速度a(X)的垂直加速度a(Z)，所述垂直加速度a(Z)在本发明中并未被使用)，始终以所述智能割草机M的车身所在平面为XOY面，如图7-1和7-2中的X轴、Y轴和Z轴，其中，车头所指方向为X轴。所述三轴角速度传感器30测得的所述三轴角速度包括三维圆坐标系下，由初步测得的航向角速度、横滚角速度转化而成的航向角Yaw与所述横滚角Roll(还包括俯仰角Pitch，所述俯仰角Pitch在本发明中并未被使用到)，所述智能割草机M在所述步骤S1中的航向角Yaw保持不变，为所述初始航向角α。
再次参见图6、图7-1、图7-2，下面主要针对所述的一种智能割草机的坡地行驶方法，对本实施例提供的一种智能割草机的坡地行驶控制技术方案的具体工作过程进行说明。
1.平地直线阶段，对应于所述步骤S1。图6中在①之前、⑤之后，车身平行于水平面。在这个阶段，Y轴所代表的所述航线加速度a(Y)的方向，X轴代表滑坡加速度a(X)的方向(Z轴代表所述智能割草机M车身的重力加速度反方向)。行驶时，根据所述三轴角速度传感器30的所述初始航向角α，分别调整所述左后轮行驶电机40和右后轮行驶电机50的PWM信号，控制它们的转速，保持所述智能割草机M的前进方向，保持所述航向角Yaw参数稳定在所述初始航向角α(预设值或允许有误差某个范围区间)，实现匀速行驶。当到达坡底如图6中的①处，此时所述MCU控制模块10执行所述步骤S2。
2.平稳上坡阶段，对应于所述步骤S1：①——>②。所述智能割草机M从①开始，以所述初始航向角α(在该阶段的所述航向角Yaw等于初始航向角α)开始稳定上坡，直到抵达滑坡开始点②处，在这个阶段，Y轴代表的所述航线加速度a(Y)的方向，X轴代表滑坡加速度a(X)的方向(Z轴代表垂直于车身所在平面的方向)。平稳上坡时，根据所述三轴角速度传感器30的航向角Yaw，分别调整所述左后轮行驶电机40和右后轮行驶电机50的PWM信号，控制它们的转速，维持所述智能割草机M以所述初始航向角α在坡地匀速行驶，直到到达图6中的滑坡起点②，此时所述智能割草机M开始滑坡。
3.滑坡阶段，对应于所述步骤S2：②——>③。所述智能割草机M从②开始滑坡，坐标参照与所述平稳上坡阶段不变，滑坡时，根据所述三轴角速度传感器30的航向角Yaw，分别调整所述左后轮行驶电机40和右后轮行驶电机50的PWM信号，控制它们的转速，以维持所述智能割草机M的前进方向在所述初始航向角α，但所述左前轮LF和右前轮RF不受控制，发生打滑，直到所述智能割草机M的前进方向再次回到所述初始航向角α。
4.调整阶段，对应于所述步骤S3～S5：③——>④——>⑤。所述智能割草机M从③开始调整，坐标参照与所述平稳上坡阶段或滑坡阶段不变，所述智能割草机M的行驶方向调整至与所述原始航线L垂直。参见，图8是本发明提供的图6实施例中智能割草机M在步骤S3～S5过程中的a方向与s方向的区别示意图。a方向代表所述滑坡方向的反方向，s方向代表与所述滑坡方向垂直的车头所指的行驶方向——即所述原始航线L所在方向。所述智能割草机M在③处和⑤处的车头方向一致，均为s方向。③——>④——>⑤行驶的连贯性决定了在③处需要调整行驶方向，所述智能割草机M在③处会将车头调整为与a方向一致而指向坡顶，即从s方向调整至a方向。调整前所述智能割草机M在③的临界点，a方向上的速度为零，s方向上的速度不为零，车头指向a方向；调整后所述智能割草机M在⑤的临界点，a方向上的速度为零，s方向上的速度不为零，车头指向s方向。
下面结合所述MCU控制模块10的最优调整算法对所述滑坡阶段、调整阶段进行整体运动分析。
1)所述智能割草机M滑坡产生的滑坡位移S(Hor)的计算方法。
i.所述智能割草机M从②到③过程中，任意时刻在水平方向上的水平方向加速度的计算方法。
所述智能割草机M在坡面行驶的物理模型再次参见图7-1、7-2。
根据所述三轴加速度传感器20和三轴角速度传感器30(陀螺仪)的数据，结合物理学受力分析得出所述智能割草机M滑坡时在图7-1(或7-2)水平方向任意时刻的水平方向加速度a(Hor)：
a(Hor)＝a(X)*cos(Roll) (1)
公式(1)中，a(X)为所述三轴加速度传感器20输出的滑坡加速度a(X)的数据，所述智能割草机M与水平方向的夹角θ(坡度)与所述三轴加速度传感器20(陀螺仪)输出的横滚角Roll相等，即在坡面平整的情况下，横滚角Roll＝θ。但由于坡面不平整，故以所述横滚角Roll代替θ，以保证数据的准确性。
由于所述六轴姿态传感器23的采样频率较高，故采样的数据具有实时性，所述智能割草机M在任意时刻的水平方向加速度a(Hor)通过公式(1)计算已得。
ii.所述智能割草机M从②到③过程，在a方向上全程的平均滑坡加速度a(avg)和该过程中任意时刻的滑坡速度V(Hor)的计算方法。
所述三轴加速度传感器20的输出数据存在一个固定速率，设为fmax。根据香农采样定理，将连续的模拟信号转换成离散的数值序列，为保证模拟信号不失真，需要采样频率f大于采样信号最大频率的二倍，即f>2fmax，保证采样的传感器数据具有连续性。
根据行驶轨迹可判定，所述智能割草机M在②——>③过程中的水平方向加速度a(Hor)均等于零。
对所述三轴加速度传感器20在水平方向上的水平方向加速度a(Hor)采样，可得a(Hor)数据变化趋势大致如图9，是本发明提供的图6实施例中智能割草机在步骤S2过程中的水平方向加速度a(Hor)与时间的关系图，横轴为时间，纵轴为所述水平方向加速度a(Hor)。时间t为所述智能割草机M从②运动到③所用的时间，即所述滑坡开始时间点t1、滑坡结束时间点t2的时间间隔，即所述滑坡时间。
所述智能割草机M从②到③的过程中，所述MCU控制模块10对所述滑坡加速度a(X)和所述横滚角Roll进行采样，每一时刻的数据为实时更新。即得出从②到③每一时刻的a(Hor)的瞬时值a1(Hor)、a2(Hor)、a3(Hor)…aM(Hor)，根据参数算出a(Hor)关于时间t的平均值——平均滑坡加速度a(avg)：
由采样数据的连续性可知，a(avg)为所述智能割草机M从②到③做变速行驶的平均加速度。具有平均加速度的变速运动可等效为匀变速运动，即匀加速度运动，②到③即为等效的匀加速运动，故a(avg)为定值。如图10所示，是本发明提供的图6实施例中智能割草机在步骤S2过程中的平均滑坡加速度a(avg)与时间t的关系等效图，横轴代表时间t，纵轴代表所述平均滑坡加速度a(avg)。在图10的等效运动中，所述智能割草机M从②行驶到③，共用所述滑坡时间t，物理学对具有恒定的平均滑坡加速度a(avg)的物体关于时间区间[0，t]的定积分等于物体的速度V(Hor)。
根据牛顿-莱布尼茨公式：
和所述智能割草机M的滑坡时间t，可得在图6中任意时刻的水平方向加速度a(Hor)对应的速度V(Hor)：
上式中，a(avg)为恒定，对于时间t为常数。
所述智能割草机M从②到③的过程中，图9中的任意时刻为m，将此刻所述六轴姿态传感器23的横滚角Roll、滑坡加速度a(X)代入公式(1)，计算得出m点的瞬时加速度aM(Hor)，同时在此过程中，所述智能割草机M有记录从②至m点，每一时刻的瞬时加速度a1(Hor)、a2(Hor)、a3(Hor)…am(Hor)，因m点的任意性，可求出0-tm时间段内，所述智能割草机M的加速度平均值a'(Hor)：
将具有平均加速度的运动等效为匀变速运动，在初始速度为零的情况下，所述智能割草机M在tm时刻的滑坡速度V'(Hor)等效为：
V′(Hor)＝a′(Hor)*tm (6)
根据所述六轴姿态传感器23采样的实时性和连续性，可求得任意时刻的速度值V'(Hor)。
iii.所述智能割草机M从②到③的过程中，在a方向上的位移计算方法。
根据不定积分公式：
与牛顿-莱布尼茨公式：
与不定积分转化成定积分的公式：
再结合公式(4)，对所述智能割草机M从②到③的等效物体速度V(Hor)关于时间区间[0，t]的定积分，得出所述智能割草机M从②到③的a方向上的滑坡位移S(Hor)：
即在t时间段内，所述智能割草机M的滑坡位移S(Hor)与所述平均滑坡加速度a(avg)之间的函数关系为：
t为所述智能割草机M从②到③的滑坡时间，a(avg)为所述滑坡时间t内等效的平均滑坡加速度，已由公式(2)求出。根据传感器采样数据的实时性和连续性，可求得该过程中所述智能割草机M在a方向上的滑坡位移S(Hor)。
2)所述智能割草机M在③的状态调整。
i.分别计算所述智能割草机M在③调整状态后，a方向上和s方向上的初始速度。
从③——>⑤，需要满足两个条件，一是调整回到所述原始航线L的距离量|S(Hor)|，公式(10)中已计算得出；二是所述智能割草机M行驶方向的一致性。
ii.所述智能割草机M在③处调整状态后在a方向上的初始速度和调整过程的平均调整加速度、相关位移的计算方法。
再次参见图7-2。所述智能割草机M从一个匀速上坡的状态切换至与原状态垂直时，所述六轴姿态传感器23固定在所述智能割草机M上以接收数据，故接收数据的方向也会随所述智能割草机M的变化而改变。调整过程不断采样所述滑坡加速度a(X)和航线加速度a(Y)和所述横滚角Roll。
从③到⑤的调整过程中，所述智能割草机M的行驶方向变化了90°，将其分解为前45°和后45°这两个过程。前45°过程中，采样使用所述滑坡加速度a(X)和所述横滚角Roll，设任意时刻在a方向上的加速度为anX"(Hor)，结合公式(2)计算出任意时刻a方向上的调整加速度anX"(Hor)：
anX″(Hor)＝a(X)*cos(Roll) (12)
后45°过程中，采样使用所述航线加速度a(Y)和所述横滚角Roll，设任意时刻在a方向上的加速度为anY"(Hor)，根据公式(2)，用a(Y)替换所述滑坡加速度a(X)，计算出任意时刻a方向上的调整加速度anY"(Hor)：
anY″(Hor)＝a(Y)*cos(Roll) (13)
设整个调整过程所用时间为t0，所述智能割草机M在a方向上的调整加速度在每一时刻对应的有a1X"(Hor)、a2X"(Hor)、a3X"(Hor)…anX"(Hor)和a1Y"(Hor)、a2Y"(Hor)、a3Y"(Hor)…anY"(Hor)，根据传感器采样数据的实时性和连续性，在所述智能割草机M调整车头90°方向后，检测出整个调整过程的平均调整加速度a0"(Hor)：
对于这个过程，已知所述智能割草机M在a方向上的初始速度为零，平均调整加速度为a0"(Hor)，在t0时间后可得出当前的调整速度V0"(Hor)：
V0”(Hor)＝a0”(Hor)*t0 (15)
t0为所述智能割草机M调整90°所用的时间。V0"(Hor)即为调整90°结束后，所述智能割草机M在a方向上的瞬时速度，也为a方向上的初始速度。
a0"(Hor)为调整过程中等效的平均加速度，根据物理学中匀加速直线运动公式，在运动过程中，保持加速度不变，由位移公式：
对于所述智能割草机M调整90°状态，在a方向上，已知初始速度V0为零。结合公式(14)，令a＝a0"(Hor)，t＝t0，所述智能割草机M在a方向上的实时调整位移S0"(Hor)：
iii.所述智能割草机M调整回所述原始航线L所需要的在a方向上的剩余位移与曲线运动的要求。
根据图6，可知所述智能割草机M从③至⑤所需要的在a方向上的待调整位移S"(Hor)加上所述智能割草机M在调整过程中产生的实时调整位移S0"(Hor)等于所述智能割草机M从②运动到③的反a方向的所述滑坡位移S(Hor)。
即有：
|S"(Hor)|+|S0"(Hor)|＝|S(Hor)| (18)
位移是矢量，具有方向性，故取其模，放入等式。
所述智能割草机M回到所述原始航线L的剩余位移|S"(Hor)|：
|S"(Hor)|＝|S(Hor)|-|S0"(Hor)| (19)
根据公式(14)可得出所述智能割草机M从③至⑤所用时间为t0，在a方向上做初始速度为V0"(Hor)的匀加速直线运动的位移公式(此公式中采用的t即为t0)：
a"(Hor)是所述智能割草机M从③所在直线垂直运动到⑤所在直线，进行匀加速直线运动需要的a方向的平均加速度。已知所述智能割草机M车头朝向a方向，所述智能割草机M在仅有a"(Hor)的情况下，会做匀加速直线运动。要求曲线运动，需要在s方向上产生加速度a"(Straight)，用a"(Hor)和a"(Straight)在空间矢量上的叠加才能行驶至⑤。
3)③——>④——>⑤坡地最优调整曲线行驶的方法。
i.所述智能割草机M从③运动至⑤，a方向上任意时刻的速度和位移计算方法。
所述智能割草机M在③——>⑤两点间最优曲线的行驶物理模型如图12所示，图12是本发明提供的图6实施例中智能割草机在步骤S3～S5过程中的最优曲线的行驶物理模型图，并结合图13，图13是本发明提供的图9实施例中的智能割草机在步骤S3～S5过程中的行驶方向示意图。③至⑤的过程中，所述智能割草机M会调整90°，车头方向从原来的a方向，调整回至s方向。在这个过程中，仍需要将90°的过程分成前45°和后45°。
将所述智能割草机M在a方向上的待调整位移S"(Hor)分成无限等分：
前45°过程等分得到ΔSY1″，ΔSY2″，ΔSY3″......ΔSYN″；
后45°过程等分得到ΔSX1″，ΔSX2″，ΔSX3″......ΔSXN″。
设③至⑤过程任意时刻为n时刻，对应时刻的位移为ΔS"(n)。
n时刻对应所述智能割草机M在a方向上的加速度an"(Hor)，针对当前调整的角度，根据公式(1)、(12)、(13)，可得在两种情况下的任意时刻在a方向上的加速度an"(Hor)：
所述航线加速度a(Y)和所述滑坡加速度a(X)为三轴加速度传感器20测得的数据，所述横滚角Roll为所述三轴角速度传感器30测得的数据。
所述智能割草机M从③至⑤，根据采样数据的实时性和连续性，根据公式(21)计算出任意时刻的加速度an"(Hor)。a方向上任意时刻n的加速度an"(Hor)已知，对n时刻及n时刻(任意时刻)以前，采样每一时刻的加速度值a1"(Hor)、a2"(Hor)、a3"(Hor)…an"(Hor)，计算出所述智能割草机M在n时刻(任意时刻)在a方向上的平均加速度a"(avg)：
根据平均加速度a"(avg)可求出对应n时刻(任意时刻)的速度：
Vn″(Hor)＝a″(avg)*n (23)
即有所述智能割草机M在a方向上，任意时刻的速度Vn"(Hor)与当前时刻的平均加速度a"(avg)关于时间n的函数关系。根据所述六轴姿态传感器23采样数据的实时性和连续性，所述智能割草机M在运动过程中检测出任意时刻的速度值Vn"(Hor)。
所述智能割草机M在极短的单位时间Δt内，设ΔS"(n)为任意时刻n的位移调整变化量，设Δt＝tna-(tna-1)，tna时刻位移为Sn，tna-1时刻位移为Sn-1，设所述智能割草机M当前速度等于恒定值Vn"(Hor)，速度方向为a方向，此时对应加速度值也为恒定值an"(Hor)，对恒定值的速度在时间区间[t-1，t]积分等于位移，结合牛顿-莱布尼茨公式：
对a方向所述智能割草机M的位移调整变化量ΔS"(n)在时间区间[tn-1，tn]积分，有：
即单位时间Δt一定时，a方向上的位移调整变化量ΔS"(n)与a方向上任意时刻的速度Vn"(Hor)有如下的函数关系：
ΔS″(n)＝Vn″(Hor)Δt (26)
根据所述六轴姿态传感器23采样数据的实时性和连续性，所述智能割草机M在运动过程中检测出任意时刻的位移调整变化量ΔS"(n)。
根据所述智能割草机M从②到③的在a方向上的滑坡位移S(Hor)和在③产生的a方向的待调整位移量S0"(Hor)，对比当前累计的位移调整变化量ΔS"(n)来判定所述智能割草机M回到所述原始航线L，即满足如下关系时：
S(Hor)-S0″(Hor)-ΔS″(1)-ΔS″(2)-…-ΔS″(n)＝0 (27)
ii.所述智能割草机M从③运动至⑤，s方向上任意时刻的速度计算方法。
在图12中，将所述智能割草机M在s方向上的位移S(Straight)分成无限等分，得到ΔS1(Straight)"、ΔS2(Straight)"、ΔS3(Straight)"、……ΔSN(Straight)"，已知所述智能割草机M从③开始运动时，s方向上初速度为零，所述智能割草机M开始运动的时间设为t＝0，设任意时刻t＝ns，从③运动至⑤过程中，所述智能割草机M的车头的状态变化与所述智能割草机M车头在③调整的状态变化方向相反，故可将公式(21)的X和Y参数对调。
所述智能割草机M在s方向上ns时刻所对应的加速度ans"(Straight)：
对ns时刻及ns时刻以前，采样每一时刻的加速度值a1"(Straight)、a2"(Straight)、a3"(Straight)…an"(Straight)，计算出在n时刻的平均调整航向加速度ans"(avg)：
根据平均调整航向加速度ans"(avg)可求出对应n时刻的调整速度Vns"(Straight)：
Vns″(Straight)＝ans″(avg)*ns (30)
根据所述六轴姿态传感器23采样数据的实时性和连续性，所述智能割草机M在运动过程中检测出所述智能割草机M从③运动至⑤在s方向上任意时刻的调整速度。
iii.所述智能割草机M从③运动至⑤，s方向上任意时刻相关位移的计算方法。
ΔS(Straight)为所述智能割草机M在所述初始航线所在的s方向的单位时间Δt的位移量。s方向上，极短时间Δt内，有Δt＝tn-(tn-1)，设极短时间内所述Vns"(Straight)为恒定值，速度变化量ΔVns"(Straight)为零，平均调整航向加速度ans"(avg)为非零的恒定值。所述智能割草机M在极短时间内的位移变化量ΔS(Straight)为前一时刻的位移Sn-1与后一时刻的位移Sn的差，在s方向，极短时间Δt内，结合牛顿-莱布尼茨公式，算得所述智能割草机M在极短时间内的位移变化量ΔS(Straight)：
即单位时间Δt一定时，s方向上的位移和s方向上的速度的函数关系：
ΔS(Straight)＝Vns″(Straight)Δt (32)
根据所述六轴姿态传感器23采样数据的实时性和连续性，所述智能割草机M在运动过程中检测出任意两个连续时刻间的位移变化量ΔS(Straight)。
iv.所述智能割草机M从③至⑤，曲线行驶的方法。
如图12，是本发明实施例提供的一种智能割草机M的坡地行驶控制方法的调整阶段的最优曲线的行驶物理模型图。所述智能割草机M若要按照③——>④——>⑤的最优曲线行驶，可将曲线近似描绘成反比例曲线函数模型，可得到a方向与s方向的位移呈现函数关系，即
其中K为比例系数即为所述预设调整常数K，表现曲线的弯曲程度，可用简单常数如1来表示。
将公式(26)、(32)代入公式(33)可得出：
将公式(23)、(30)代入(34)，可得公式(35)，所有公式时间统一为Δt，化简后可得出，在极短的单位时间Δt内，所述智能割草机M在坡地行驶最优曲线回到所述原始航线L，在s方向的目标调整航向加速度ans"(avg)*：
上式表示a方向与s方向，两者加速度的函数关系。Δt为单位时间的变化量，即为时间间隔，与传感器采样数据间隔时间相同。保证这个函数关系，所述智能割草机M即沿最优曲线：③——>④——>⑤行驶。
行驶过程中，所述智能割草机M根据公式(21)，检测并记录a方向的加速度an"(Hor)和s方向上的加速度ans"(Straight)，根据公式(22)计算出a方向的平均加速度a"(avg)，根据公式(29)计算出当前时刻的s方向的平均调整航向加速度ans"(avg)(实际值)。将a方向的平均加速度a"(avg)代入公式(35)，计算出当前时刻的目标调整航向加速度ans"(avg)*(理论值)。再根据ans"(avg)*与ans"(avg)之间差值的正负，调整所述左后轮行驶电机40与右后轮行驶电机50的PWM比值，由比值产生对应的左右轮转速差，实现a方向和s方向上的加速度改变。在下一时刻的时候，再根据下一时刻的ans"(avg)*与ans"(avg)的比较结果，决定左右轮转速。总体来说，即是根据调整航向加速度的实际值与理论值的偏差以实时调整左右轮转速。
再次参见图13，所述智能割草机M执行曲线行驶，在③处，检测得到当前的航线加速度a(Y)和所述滑坡加速度a(X)、横滚角Roll，将公式(35)和公式(29)的运算结果对比大小，当公式(35)结果大于公式(29)结果时，即所述目标调整航向加速度ans"(avg)*大于所述平均调整航向加速度ans"(avg)时，下一时刻提高外侧轮转速并大于内侧轮的转速，此时s方向的速度增加以保持图13中的最优曲线行驶。当公式(35)结果小于或等于公式(29)时，即所述目标调整航向加速度ans"(avg)*小于或等于所述平均调整航向加速度ans"(avg)时，下一时刻外侧轮转速应该等于(或小于)内侧轮转速，这时s方向的速度会相对减少以保持图13中的最优曲线行驶。这里的内侧轮指的转弯行走时，靠近弯道内侧的左后轮LR或右后轮RR，对应地，另一个后轮即为外侧轮。
为了突出公式(1)～(35)中部分公式的重要性，参见，图14是本发明实施例提供的图4实施例的算法程序图，在图4实施例的基础上特别增加了推导出所述滑坡位移S(Hor)、所述平均滑坡加速度a(avg)、所述水平滑坡加速度a(Hor)、所述实时调整位移S0"(Hor)、所述平均调整航向加速度ans"(avg)、所述目标调整航向加速度ans"(avg)*的重要公式：公式(2)、公式(11)～(14)、公式(17)、公式(21)～(23)、公式(26)～(30)、公式(32)～(34)。
本发明实施例提供的一种智能割草机的坡地行驶控制方法与系统，在智能割草机M发生滑坡偏离原始航线L后，能够实时规划出最优的行驶路线，使得偏离的智能割草机M能够最优地驶回原始航线L，同时避免出现智能割草机M因坡地因素在多坡地段重复路线割草的问题，极大地提高了割草的覆盖率，基本实现无割草盲区。本发明提供的一种智能割草机的坡地行驶控制系统，设置的高可靠性的三轴加速度传感器20、三轴角速度传感器30及高精度的MCU控制模块10使得智能割草机M反应更灵敏、割草更精准、智能化程度更高，能够极大地提高智能割草机M对复杂工作环境的适应性，拓宽其适用范围。
上述实施例为本发明较佳的实施方式，但本发明的实施方式并不受上述实施例的限制，其他的任何未背离本发明的精神实质与原理下所作的改变、修饰、替代、组合、简化，均应为等效的置换方式，都包含在本发明的保护范围之内。
Claims (9)
1.一种智能割草机的坡地行驶控制方法，其特征在于，包括以下步骤：
S1.根据预设的初始航向角控制所述智能割草机在原始航线上匀速直线行驶；
S2.检测是否存在滑坡行驶的标志位；若否，则处理当前采集的三轴角速度和三轴加速度，并在判定所述智能割草机结束滑坡时生成所述滑坡行驶的标志位和计算得到滑坡位移；若是，则执行步骤S3；
S3.根据计算获得的与所述滑坡行驶的标志位对应的滑坡方向，将所述智能割草机的行驶方向调整至与所述原始航线垂直并实时记录采集获得的所述三轴角速度和所述三轴加速度；
S4.根据当前记录的所述三轴加速度和所述三轴角速度，计算得到所述智能割草机在所述滑坡方向上的实时调整位移；
S5.根据实时记录的所述三轴角速度和所述三轴加速度，实时调整所述智能割草机的行驶方向和行驶速度，直到所述实时调整位移为零值时，对所述滑坡位移和所述滑坡行驶的标志位清零，并返回到所述步骤S1。
2.如权利要求1所述的一种智能割草机的坡地行驶控制方法，其特征在于，在所述步骤S2中，处理当前采集的三轴角速度和三轴加速度，并在判定所述智能割草机结束滑坡时生成所述滑坡行驶的标志位和计算得到滑坡位移，具体包括：
S2-1.根据实时采集的所述三轴加速度在所述滑坡方向上的滑坡加速度，判断所述智能割草机是否在滑坡行驶；若是，则执行步骤S2-2；若否，则执行步骤S2-3；
S2-2.记录滑坡开始时刻，根据当前记录的三轴角速度和三轴加速度计算并保存所述智能割草机从所述滑坡开始时刻起在所述滑坡方向上的滑坡加速度，并返回到所述步骤S1；
S2-3.检测所述智能割草机在水平面上是否存在水平滑坡加速度；若否，则返回到所述步骤S1；若是，则执行步骤S2-4；
S2-4.记录滑坡结束时刻，计算得到滑坡时间，并根据所述滑坡时间和在所述滑坡时间内的所述水平滑坡加速度，计算并记录平均滑坡加速度和滑坡位移；
S2-5.清零所述水平滑坡加速度，生成所述滑坡行驶的标志位。
3.如权利要求2所述的一种智能割草机的坡地行驶控制方法，其特征在于，所述步骤S5具体包括：
S5-1.根据预设的时间间隔，对每一时刻的所述三轴加速度中的所述滑坡加速度、所述原始航线所在方向上的航向加速度和所述三轴角速度中的横滚角进行实时采样，计算得到各个时间间隔内的位移调整变化量，并进一步计算得到当前时刻的平均调整航向加速度、平均目标航向加速度；
S5-2.判断所述滑坡位移与当前各个时间间隔内累计获得的所述实时调整位移是否大小相同；若是，则对所述滑坡位移和所述滑坡行驶的标志位清零，并返回到所述步骤S1；若否，则执行步骤S5-3；
S5-3.判断所述平均调整航向加速度与所述平均目标航向加速度是否相同；若是，则通过PWM信号控制所述智能割草机的外轮转速大于内轮转速；若否，则通过PWM信号控制所述智能割草机的外轮转速小于内轮转速；
S5-4.返回到所述步骤S5-1。
4.一种智能割草机的坡地行驶控制系统，其特征在于，包括MCU控制模块及与其连接的三轴加速度传感器和三轴角速度传感器；
所述三轴加速度传感器和三轴角速度传感器分别用于实时采集所述智能割草机的三轴加速度和三轴角速度，并将所述三轴加速度和所述三轴角速度发送到所述MCU控制模块；
所述MCU控制模块设有平稳行驶驱动子模块、标志位检测与处理子模块、滑坡调整驱动子模块；
所述平稳行驶驱动子模块，用于根据预设的初始航向角控制所述智能割草机在原始航线上匀速直线行驶；
所述标志位检测与处理子模块，用于在未检测到有滑坡行驶的标志位时，处理所述三轴加速度传感器当前采集的三轴加速度和所述三轴角速度传感器当前采集的三轴角速度，并在判定所述智能割草机结束滑坡时，生成所述滑坡行驶的标志位和计算得到滑坡位移；
所述标志位检测与处理子模块，还用于在检测到存在滑坡行驶的标志位时，计算获得与所述滑坡行驶的标志位对应的滑坡方向；
所述滑坡调整驱动子模块，用于根据所述滑坡方向将所述智能割草机的行驶方向调整至与所述原始航线垂直，并实时记录采集获得的所述三轴角速度和所述三轴加速度；
所述滑坡调整驱动子模块，还用于处理当前记录的所述三轴加速度和所述三轴角速度，计算得到所述智能割草机在所述滑坡方向上的实时调整位移；
所述滑坡调整驱动子模块，还用于根据实时记录的所述三轴角速度和所述三轴加速度，实时调整所述智能割草机的行驶方向和行驶速度，直到所述实时调整位移为零值时，对所述滑坡位移和所述滑坡行驶的标志位清零。
5.如权利要求4所述的一种智能割草机的坡地行驶控制系统，其特征在于：所述三轴加速度包含所述智能割草机在所述原始航线所在方向上的航向加速度、在所述滑坡方向上的滑坡加速度；所述三轴角速度包含航向角速度、横滚角速度，所述航向角速度、横滚角速度转化为航向角和横滚角，所述航向角包含所述初始航向角。
6.如权利要求5所述的一种智能割草机的坡地行驶控制系统，其特征在于：所述标志位检测与处理子模块设有滑坡行驶判定与处理子模块、数据记录与清零子模块，以及，标志位生成子模块；
所述滑坡行驶判定与处理子模块，用于在判定所述智能割草机在滑坡行驶时，根据实时采集的所述三轴加速度在所述滑坡方向上的滑坡加速度，先后驱动所述数据记录与清零子模块、所述平稳行驶驱动子模块工作；在判定所述智能割草机未在滑坡行驶、并检测到所述智能割草机在水平面上存在水平滑坡加速度时，驱动所述数据记录与清零子模块工作；在判定所述智能割草机未在滑坡行驶、并检测到未有记录的所述智能割草机在水平面上的水平滑坡加速度时，驱动所述平稳行驶驱动子模块工作；
所述数据记录与清零子模块，用于响应所述滑坡行驶判定与处理子模块的驱动，记录滑坡开始时刻，根据当前记录的三轴角速度和三轴加速度计算并保存所述智能割草机从所述滑坡开始时刻起在所述滑坡方向上的滑坡加速度，并驱动所述平稳行驶驱动子模块工作；记录滑坡结束时刻，计算得到滑坡时间，并根据所述滑坡时间和在所述滑坡时间内的所述水平滑坡加速度，计算并记录得到平均滑坡加速度和滑坡位移，清零所述水平滑坡加速度，驱动所述标志位生成子模块工作；
所述平稳行驶驱动子模块，用于响应所述滑坡行驶判定与处理子模块及所述数据记录与清零子模块的驱动，根据所述预设的初始航向角控制所述智能割草机在所述原始航线上匀速直线行驶；
所述标志位生成子模块，用于响应数据记录与清零子模块的驱动，生成所述滑坡行驶的标志位。
7.如权利要求6所述的一种智能割草机的坡地行驶控制系统，其特征在于：所述滑坡调整驱动子模块设有加速度计算子模块、判断与处理子模块和车轮驱动子模块；
所述加速度计算子模块，用于根据预设的时间间隔，对每一时刻的所述三轴加速度中的所述滑坡加速度、所述航向加速度和所述三轴角速度中的横滚角进行实时采样，计算得到各个时间间隔内的位移调整变化量，并进一步计算得到当前时刻的平均调整航向加速度、平均目标航向加速度；
所述判断与处理子模块，用于判定所述滑坡位移与当前各个时间间隔内的位移调整变化量累计的所述实时调整位移之间的差值大于0时，驱动所述数据记录与清零子模块工作；
所述数据记录与清零子模块，还用于响应所述判断与处理子模块的驱动，对所述滑坡位移和所述滑坡行驶的标志位清零；
所述判断与处理子模块，还用于在判定所述滑坡位移与当前各个时间间隔内累计的所述实时调整位移之间的差值小于或等于0，并且所述平均调整航向加速度与所述平均目标航向加速度之间的差值大于0时，驱动所述车轮驱动子模块工作；或者，在判定所述平均调整航向加速度与平均目标航向加速度之间的差值小于或等于0时，驱动所述车轮驱动子模块工作；
所述车轮驱动子模块，用于响应所述判断与处理子模块的驱动，通过PWM信号控制所述智能割草机的外轮转速大于内轮转速；或者，通过PWM信号控制所述智能割草机的外轮转速小于内轮转速。
8.如权利要求7所述的一种智能割草机的坡地行驶控制系统，其特征在于：还包括独立连接所述车轮驱动子模块的左后轮行驶电机和右后轮行驶电机；所述左后轮行驶电机，用于接收所述车轮驱动子模块发出的PWM信号，独立调整所述智能割草机的左后轮转速；所述右后轮行驶电机，用于接收所述车轮驱动子模块发出的PWM信号，独立调整所述智能割草机的右后轮转速。
9.如权利要求8所述的一种智能割草机的坡地行驶控制系统，其特征在于：所述智能割草机的左前轮和右前轮为采用支撑结构的万向轮。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201710790701.3A CN107509443B (zh) | 2017-09-05 | 2017-09-05 | 一种智能割草机的坡地行驶控制方法及系统 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201710790701.3A CN107509443B (zh) | 2017-09-05 | 2017-09-05 | 一种智能割草机的坡地行驶控制方法及系统 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN107509443A CN107509443A (zh) | 2017-12-26 |
| CN107509443B true CN107509443B (zh) | 2020-01-17 |
Family
ID=60724942
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201710790701.3A Active CN107509443B (zh) | 2017-09-05 | 2017-09-05 | 一种智能割草机的坡地行驶控制方法及系统 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN107509443B (zh) |
Families Citing this family (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112578777A (zh) * | 2019-09-27 | 2021-03-30 | 苏州宝时得电动工具有限公司 | 自主机器人及其行走路径规划方法、装置和存储介质 |
| CN111561931A (zh) * | 2020-05-09 | 2020-08-21 | 深圳拓邦股份有限公司 | 移动机器人的路径规划方法、装置及计算机可读存储介质 |
| WO2021254462A1 (zh) * | 2020-06-18 | 2021-12-23 | 南京德朔实业有限公司 | 割草机器人 |
| CN111880529B (zh) * | 2020-06-29 | 2021-11-12 | 东风商用车有限公司 | 基于高精度地图的坡道巡航车速控制方法 |
| CN112859846B (zh) * | 2021-01-05 | 2023-03-10 | 海门市帕源路桥建设有限公司 | 一种自动水平行走控制方法 |
| CN114568108B (zh) * | 2022-02-28 | 2022-11-11 | 清华大学深圳国际研究生院 | 一种无人割草机轨迹跟踪控制方法及计算机可读存储介质 |
Family Cites Families (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JPH0974849A (ja) * | 1995-09-08 | 1997-03-25 | Nikkari Co Ltd | 斜面用草刈機装置 |
| US8060269B2 (en) * | 2008-04-16 | 2011-11-15 | Cnh America Llc | Swath line creation including slope compensation for an automatic guidance system of a work vehicle |
| US9072218B2 (en) * | 2012-07-09 | 2015-07-07 | Deere & Company | Boundary sensor assembly for a robotic lawn mower, robotic lawn mower and robotic lawn mower system |
| WO2014129941A1 (en) * | 2013-02-20 | 2014-08-28 | Husqvarna Ab | A robotic work tool configured for improved turning in a slope, a robotic work tool system, and a method for use in the robot work tool. |
| SE539760C2 (en) * | 2014-12-23 | 2017-11-21 | Husqvarna Ab | Control of downhill movement for a robotic work tool |
| CN106325287A (zh) * | 2015-06-14 | 2017-01-11 | 复旦大学 | 一种基于惯性/磁传感器marg姿态检测的智能割草机直线行走控制系统 |
| CN106325276B (zh) * | 2016-09-21 | 2019-06-25 | 苏州瑞得恩光能科技有限公司 | 机器人在斜坡平面上直线行驶的判定方法及控制方法 |
-
2017
- 2017-09-05 CN CN201710790701.3A patent/CN107509443B/zh active Active
Also Published As
| Publication number | Publication date |
|---|---|
| CN107509443A (zh) | 2017-12-26 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN107509443B (zh) | 一种智能割草机的坡地行驶控制方法及系统 | |
| Amidi et al. | Integrated mobile robot control | |
| US11027415B1 (en) | Generalized coordinate surrogates for integrated estimation and control | |
| AU2006274420B2 (en) | Guidance, navigation, and control system for a vehicle | |
| US8160809B2 (en) | Close-spaced leader-follower navigation using control mimic | |
| CN110243358A (zh) | 多源融合的无人车室内外定位方法及系统 | |
| US10214223B1 (en) | Method and apparatus for determining vehicle location based on motor feedback | |
| KR20210084526A (ko) | 로봇 운동시 카펫으로 인한 드리프트 제어방법, 칩 및 청소 로봇 | |
| CN106292741A (zh) | 一种基于无刷电机的移动机器人云台系统 | |
| CN107063305B (zh) | 用惯导、压力传感器修正下坡悬空后轮里程计误差的方法 | |
| CN110001840B (zh) | 一种基于视觉传感器的多种路况下的两轮自平衡车运动控制方法 | |
| CN107600267B (zh) | 一种二轮单轨车辆及其平衡的控制方法 | |
| US9043097B2 (en) | System and method for estimating machine pitch angle | |
| CN105737853B (zh) | 一种机器人惯性导航系统的漂移校准方法 | |
| CN111474938A (zh) | 一种惯性导航自动导引小车及其航迹确定方法 | |
| CN106325287A (zh) | 一种基于惯性/磁传感器marg姿态检测的智能割草机直线行走控制系统 | |
| CN112923931A (zh) | 一种基于固定路线下的特征地图匹配与gps定位信息融合方法 | |
| CN109656240A (zh) | 一种车辆路径跟踪控制装置、方法和车辆 | |
| Song et al. | A new method of AGV navigation based on Kalman Filter and a magnetic nail localization | |
| CN108189637A (zh) | 一种应急救援车辆主动悬挂作动器控制量的数据融合方法 | |
| CN109901602A (zh) | 一种菱形四轮中耕机主动调平系统及方法 | |
| CN108507572A (zh) | 一种基于mems惯性测量单元的姿态定位误差修正方法 | |
| CN115303265A (zh) | 车辆避障控制方法、装置及车辆 | |
| CN108196545B (zh) | 采用自抗扰控制技术的agv磁导航控制方法 | |
| CN109541963B (zh) | 一种基于侧滑角信息的无人机测风建模方法 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |