CN103487812B - 一种温室自主移动车辆的超声波导航装置及方法 - Google Patents

Abstract

本发明公开了一种温室自主移动车辆的超声波导航装置及方法。前后万向轮可以辅助自主移动车辆的履带驱动轮在狭窄垄间实施往返的行走，并能适应温室垄间地形，通过自动调整左右履带轮的速度可以实现移动车辆在垄埂末端小半径原地直角转向。安装在车上的六个超声波测距传感器实时发射超声波，经接收来自垄间墙壁反射的超声波可以计算出移动车辆当前航向偏差与航向偏角，再通过ARM控制器调整左右履带轮的速度消除航向偏差与航向偏角，从而实现自主移动车辆在垄间的自主导航控制。本发明能适应温室垄间的地形，可有效解决在温室狭窄垄间实施往返行走和小半径原地直角转向的问题，可应用于温室的智能化作业。

Description

一种温室自主移动车辆的超声波导航装置及方法

技术领域

本发明属于温室设施技术领域，具体涉及一种温室设施导航技术。

背景技术

设施农业在我国农业领域具有重要的地位，我国连栋温室的面积处于世界前列。为节约劳动力、减轻劳动强度、提高作业效率，温室内智能化作业机具日益受到科技人员的重视。温室内作业项目涉及到每日的作物信息采集、作物保护管理、果实收获等，在大面积的连栋温室内利用智能化自主移动车辆有助于这些作业的高效完成。导航技术作为智能移动车辆的核心技术已成为研究的热点，目前自主式移动车辆的导航方式主要有机器视觉导航，电磁导航，光学导航，激光导航等。机器视觉导航是通过获取车辆前方目标背景的图像，并从中提取导航路径；它易于实现信息的集成，但当受到温室内目标背景信息叠加多义的影响时，使得获取目标背景相当困难。电磁导航是在移动车辆行走的路径上埋设金属线，移动车辆检测通电的金属线所产生的磁场以获取行走的路径；它结构简单，成本低，但是路径难以更改扩张。光学导航是在移动车辆行走的路径上粘贴色带，通过摄像机采集色带图像信号进行简单的处理而实现导引；它设置简单，但是色带易于磨损。激光导引是在移动车辆的路径的周围安装精确的激光反射板，移动车辆通过采集反射板反射回的激光束获取当前的位置与方向；它导引定位精度高，但是成本较高，在温室内实施时，信号易受到立柱，吊绳，作物等物件的遮挡。

中国发明专利ZL200310111146.5，发明名称为“光学制导式自动导引车导引装置”。该发明专利公开了一种自动导引车导引装置包含主控制器，转向控制装置及行走控制装置。主控制器前安装的激光扫描头按控制周期以固定的速度旋转扫描，与激光扫描头等高的激光反射板通过对反射回的激光信号进行处理，并计算出当前AGV的位姿，再经主控制器处理比较当前位姿与目标位姿信息分析得出理想的转向控制信号与行走控制信号，分别输送到转向伺服放大器和行走伺服放大器，并由其控制转向电机与驱动电机工作，以实现一定范围有规则的导引。该光学导引方法实现成本高，针对温室垄间狭窄复杂的环境，激光易受到温室立柱、吊绳、作物等物体的遮挡，可靠性降低，实现相对复杂。

中国发明专利ZL200910104460.8，发明名称为“一种基于颜色路标的移动机器人视觉导航方法”。该发明通过设计带有两个不同颜色环的圆柱体作为机器人视觉导航的目标，并在HIS颜色空间下实现有效彩色路标分割，并通过色环的图像信息，实现机器人对目标的识别，再采用灰度相关的匹配法和粒子群算法分别优化两色环中心距离和路标全局坐标，最终实现对路标精确定位与视觉导航。在温室内，室内设施与作物密度大，植株之间遮挡可能影响对颜色环的捕捉，这就使得该视觉导航方法很难获取精确的路标信息。

中国实用新型专利ZL201220163645.3，实用新型名称为“一种基于视觉和超声波的导航控制***”。该专利公开了一种基于视觉和超声波的导航控制***，包括履带式式机器人本体，设置在位于机器人前方的超声波测距子***，和正上方的视觉子***以及位于机器人上方的运动控制子***。履带式机器人在移动过程中，视觉子***和超声波测距子***协同工作。视觉子***的CCD摄像机实时采集机器人前方的路况视频信息，通过模数转换电路将该视频信号转换成数字信号，由视频口传送给DSP图像处理器，获取导航的参数，并将视觉子***得到的信息通过DSP运动控制器的UART串口传输到运动控制子***中；同时超声波谐振频率发生电路，调理电路产生超声波信号，由超声波接收器接收的回波信号经处理输送到运动控制子***。由视觉子***和超声波子***获取的经处理后的信息传输到嵌入在运动控制子***中的模糊控制器，再经控制器输出控制信息控制机器人运动。超声波获取的导航信息与视觉获取的导航信息属于相互不交融的信息，实现不同种类的信息的融合固然可以提高导航的精度，但是不同类信息融合处理相对繁琐，速度慢，实时性差，针对温室复杂的环境，视觉信号处理更加复杂且精度低，基于视觉和超声波的导航控制方式的缺点更加明显。

参与文献

[1]发明专利ZL200310111146.5，发明名称为“光学制导式自动导引车导引装置”。

[2]发明专利ZL200910104460.8，发明名称为“一种基于颜色路标的移动机器人视觉导航方法”。

[3]实用新型专利ZL201220163645.3,实用新型名称为“一种基于视觉和超声波的导航控制***”。

发明内容

本发明的目的在于提供一种温室自主移动车辆的超声波导航装置及方法，以适应温室作业环境中存在室内附属设施多、作物栽培密度大、空间利用率高、目标背景信息叠加多义等温室复杂环境特点，实现在温室狭窄垄间往返行走、小半径原地直角转向、并能适应温室垄间地形的的自主移动车辆装置和高效的导航控制。

为了解决以上技术问题，本发明针对垄埂种植模式的作物，针对温室内的环境与作业移动车辆垄间行走工作的特点，提出了一种应用于温室的自主移动车辆超声波导航装置，并述其具体实施方法，具体技术方案如下：

一种应用于温室的自主移动车辆超声波导航装置，包括自主移动车辆与安装在车辆外侧靠近车厢底板的六测量点处的六个超声波测距传感器(7)；

所述自主移动车辆由车厢(1)、ARM嵌入式控制器(2)、左编码器(11)、右编码器(3)、左直流伺服电机(10)、右直流伺服电机(4)、蓄电池组(6)、6个信号调理与变送模块(8)、左伺服电机驱动器(9)、右伺服电机驱动器(5)、无线数传模块(12)、2个前万向轮(21)、2个后万向轮(14)、4个内筒(15)、4个弹簧(16)、4个外筒(17)、2个履带(18)、2个大履带轮(20)、2个小履带轮(19)和温度检测模块(13)组成；

所述车厢(1)的右侧两测量点处安装有一对超声波测距传感器A(71)和超声波测距传感器B(72)、左侧两测量点处安装一对超声波测距传感器D(74)和超声波测距传感器E(75)，每对超声波测距传感器之间距离为3/4的车身长度且关于车体纵向中心面对称；所述车厢(1)的前侧的测量点处超声波测距传感器C(73)和后侧的两测量点处超声波测距传感器F(76)分别安装在车体纵向中心面上；

所述固定在车厢(1)内的ARM嵌入式控制器(2)经RS485通信总线以串行方式与左伺服电机驱动器(9)和右伺服电机驱动器(5)相连；左伺服电机驱动器(9)引出两条导线，一条与左直流伺服电机(10)相连，一条与左编码器(11)相连；右伺服电机驱动器(5)引出两条导线，一条与右直流伺服电机(4)相连，一条与右编码器(3)相连；

所述ARM嵌入式控制器(2)经RS232的通信总线分别连接信号调理与变送模块(8)和无线数传模块(12)，6个信号调理与变送模块(8)分别直接与6个超声波测距传感器(7)一一相连；所述左编码器(11)和右编码器(3)外壳分别与左直流伺服电机(10)和右直流伺服电机(4)外壳固定，其转子与对应的伺服电机转子采用柔性连接器相互连接；所述温度检测模块(13)通过自带接口与ARM嵌入式控制器(2)相连；

所述固定在车厢(1)侧壁上的左直流伺服电机(10)和右直流伺服电机(4)其输出轴经键与小履带轮(19)相互连接，小履带轮(19)经履带(18)与可转动的安装在车厢(1)的侧壁上大履带轮(20)链接，且大履带轮(20)与小履带轮(19)轴线所在平面与车厢(1)底板所在平面垂直；

4个外筒(17)通过固定在四个对称分布在车厢(1)两侧的安装凸台上，所述外筒(17)为空心的，A端开口，且有另一端面B；4个内筒(15)的端面A分别可移动地内套于4个对应的外筒(17)内，4个内筒(15)的另一端面B分别与2个前万向轮(21)和2个后万向轮(14)焊接在一起并从外筒(17)的A端伸出；内筒(15)的端面A与外筒(17)的端面B之间通过弹簧(16)连接。

一种利用所述的应用于温室的自主移动车辆超声波导航装置的导航方法，其特征在于包括以下步骤：

步骤1，上位机通过无线数传模块向自主移动车辆发射启动请求信号，移动车辆接收到请求信号后，ARM嵌入式控制器初始化，启动驱动程序，设置导航***总定时器时间为T₁，使每隔时间T₁自主移动车辆向四周发射超声波，考虑到自主移动车辆与左右垄侧墙壁的距离狭窄，设定T₁在0.1s—0.5s之间，此时直接启动驱动程序，驱动电机运动带动车辆前进；

步骤2，移动车辆向前移动时，总定时器从零计时，到达时间T₁时，进入定时中断子程序，设置所有超声波发射装置的初始时间为t₀，ARM嵌入式控制器经驱动电路控制超声波测距传感器(7)发射超声波，所有超声波发射装置发射完成后，停止发射，返回程序；

步骤3，当超声波测距传感器A(71)、超声波测距传感器B(72)、超声波测距传感器C(73)、超声波测距传感器D(74)、超声波测距传感器E(75)和超声波测距传感器F(76)开始接收到回波时，此处设等待接收超声波时间为T₂，考虑到超声波传播速度与狭窄垄间的距离，设定T₂在0.02s—0.08s之间，在自主移动车辆的前后方无障碍时，超声波测距传感器C(73)和超声波测距传感器F(76)检测不到回波时，读取时间自动赋值为T₂，等待时间结束后，启动外部中断子程序，关闭所有的超声波发射装置中断定时器；读取超声波测距传感器A(71)、超声波测距传感器B(72)、超声波测距传感器C(73)、超声波测距传感器D(74)、超声波测距传感器E(75)和超声波测距传感器F(76)接收到超声波回波后的记录时间分别为t₁、t₂、t₃、t₄、t₅、t₆，这样6个超声波测距传感器从发射超声波到接收超声波的时间依次分别为(t₁-t₀)、(t₂-t₀)、(t₃-t₀)、(t₄-t₀)、(t₅-t₀)、(t₆-t₀)；

为补偿温室温度对超声波在空气中速度的影响，所安装的温度检测模块能实时的检测出当前的温室的温度T，则超声波速度在温室中的传播速度为：

$V=331.4\sqrt{1+\frac{T}{273}}$

步骤4，计算自主移动车辆各超声波测距传感器安装的测量点处与对应垄侧墙壁的距离为：

D1＝V×(t₁-t₀)/2

D2＝V×(t₂-t₀)/2

D3＝V×(t₃-t₀)/2

D4＝V×(t₄-t₀)/2

D5＝V×(t₅-t₀)/2

D6＝V×(t₆-t₀)/2

步骤5，计算自主移动车辆当前的航向偏差ΔX与航向偏角Δθ

$\mathrm{\α}=\arctan \frac{D4-D3}{D0}$

$\mathrm{\β}=\arctan \frac{D1-D2}{D0}$

$\mathrm{\Δ\θ}=\frac{\mathrm{\α}+\mathrm{\β}}{2},\mathrm{\δ}=\mathrm{\Δ\θ}$

$\mathrm{\ΔX}=\frac{|(D3+D4)/2|-|(D1+D2)/2|}{2}\cos \mathrm{\Δ\θ}$

步骤6，在自主移动车辆前超声波测距传感器所测自主移动车辆前测量点与障碍的实际距离大于等于自主移动车辆前测量点与障碍物最小安全距离且后超声波测距传感器所测自主移动车辆后测量点与障碍的实际距离大于等于自主移动车辆后测量点与障碍物最小安全距离情况下，当Δθ＝0且ΔX＝0时，继续运行左右伺服电机，无需角度调整；而当Δθ≠0或ΔX≠0时，ARM嵌入式控制器通过左右伺服电机驱动器调整左右伺服电机速度，以消除前进的航向偏差与偏角，以自主移动车辆前进方向为基准，且定义Δθ＞0时，移动车辆当前方向是向右偏的，移动车辆上的ARM控制器控制左右伺服电机速度进行左转向调整，Δθ＜0时，与Δθ＞0时情况相反；以自主移动车辆前进方向为基准，且定义ΔX＞0时，小车重心与右垄靠近，移动车辆需要左移调整，ΔX＜0时，与ΔX＞0的情况相反；在自主移动车辆前超声波测距传感器所测自主移动车辆前测量点与障碍的实际距离小于自主移动车辆前测量点与障碍物最小安全距离或后超声波测距传感器所测自主移动车辆后测量点与障碍的实际距离大于等于自主移动车辆后测量点与障碍物最小安全距离情况下，ARM嵌入式控制器控制驱动电机停止转动，自主移动车辆垄间准备后退时，在小车后退的过程中，超声波检测的方法与自主移动车辆前进时相同。

步骤7，自主移动车辆自身调整完成后，将相关运行状态经无线数传模块反馈到上位机，此时自主移动车辆总定时时间设定为0，返回步骤1。

本发明的工作过程如下：所设自主移动车辆可适应温室垄间地形，能在狭窄垄间实施往返的行走，并通过自动调整左右履带轮的速度可以实现移动车辆在垄埂末端小半径原地直角转向。安装在车上测量点处的六个超声波测距传感器实时发射超声波，经接收来自垄间墙壁反射的超声波可以计算出移动车辆当前航向偏差与航向偏角，再通过ARM嵌入式控制器调整左右履带轮的速度消除航向偏差与航向偏角，从而实现自主移动车辆在垄间的自主导航控制。

安装在自主移动车辆前方的超声波测距传感器C73用于检测自主移动车辆前测量点与自主移动车辆前方存在障碍的距离，超声波测距传感器F76用于检测自主移动车辆后测量点与自主移动车辆后方存在障碍的距离，安装在自主移动车辆左右超声波测距传感器A71、超声波测距传感器B72、超声波测距传感器D74和超声波测距传感器E75用于检测自主移动车辆左右测量点与对应左右垄侧的距离。每个超声波传感器包括超声波发射器，超声波接收器，超声波驱动电路与超声波接收电路组成。在自主移动车辆开始工作时，固定在车厢内部的蓄电池组6通过导线给各部件提供电源，在ARM嵌入式控制器2给出发射超声波信号，超声波驱动电路发出经过滤波、整形，放大后的方波信号，超声波发射器发射指定频率的超声波，经过反射后的超声波经超声波接收器接收并经超声波接收电路处理传送到对应的6个信号调理与变送模块8。由于温室内温度与室外温度的差异，所设温度检测模块2，实时检测温室内温度传送给ARM嵌入式控制器2以补偿温度对计算造成的影响，6个信号调理与变送模块8经过处理超声波测距传感器A71、超声波测距传感器B72、超声波测距传感器C73、超声波测距传感器D74、超声波测距传感器E75、超声波测距传感器F76所检测的信息，经RS232总线直接传达给ARM嵌入式控制器2，并由其得出当前自主移动车辆的航向偏差与偏角，再由RS485通信总线控制左伺服电机驱动器9和右伺服电机驱动器5，以此来控制左直流伺服电机10和右直流伺服电机4的速度消除偏差与偏角。

本发明具有有益效果。本发明针对温室内复杂作业环境，所研究温室自主移动车辆超声波导航装置和实施方法可以有效解决在温室狭窄垄间实施往返行走和小半径原地直角转向的问题，并且该装置可以较好的适应温室垄间的地形，这为温室移动车辆在垄间的高效作业提供便利，从而提高温室自主行走车辆工作效率，有助于实现温室内智能化作业。

附图说明

图1是本发明应用自主移动式车辆俯视结构示意图；

图2是本发明应用自主移动式车辆主视结构示意图；

图3是本发明应用自主移动式车辆辅助转向装置剖视图；

图4是本发明实施例总体结构框图；

图5是本发明自主式移动车辆垄间行走示意图；

图6是本发明自主式移动车辆垄端转向示意图；

图7是本发明主程序控制流程图；

图8是本发明控制定时中断流程图；

图9是本发明控制外部中断流程图；

图10是本发明驱动程序流程图。

图中：1.车厢，2.ARM嵌入式控制器，3.右编码器，4.右直流伺服电机，5.右伺服电机驱动器，6.蓄电池组，7.超声波测距传感器，71.超声波测距传感器A，72.超声波测距传感器B，73.超声波测距传感器C，74.超声波测距传感器D，75.超声波测距传感器E，76.超声波测距传感器F，8.信号调理与变送模块，9.左伺服电机驱动器，10.左直流伺服电机，11.左编码器，12.无线数传模块，13.温度检测模块，14.后万向轮，15内筒，16.弹簧，17.外筒，18.履带，19.小履带轮，20.大履带轮，21.前万向轮，22.垄尽头墙壁，23.垄侧墙壁。

具体实施方式

下面结合附图，对本发明的技术方案做进一步详细说明。

本实施例自主移动车辆超声波导航装置包含自主移动车辆和安装在车辆外侧靠近车厢1底板六测量点处的六个超声波测距传感器7，如图1，图2和图3所示。自主移动车辆由车体平台和控制***组成，车体平台包含车厢1以及安装在车厢上四个对称分布于车厢前后两端的辅助转向装置和对称分布于车体中部的两个完全相同的驱动总成。车厢1是由铝型材搭建而成的矩形框架，用于承载自主车辆的各种设备和装置。以左驱动总成为例，左驱动总成包括左直流伺服电机10、履带18、小履带轮19和大履带轮20，左直流伺服电机10是通过螺栓螺母固定在车厢1的侧壁上，小履带轮19经键固定在左直流伺服电机10的输出轴上，大履带轮20与小履带轮19轴线所在平面与车厢1底板所在平面垂直，且大履带轮20可转动的安装在车厢1的侧壁上，经履带18与小履带轮19链接。以后辅助转向装置为例，后辅助转向装置包括后万向轮14，内筒15，外筒17和弹簧16，外筒17通过螺栓螺母固定在四个对称分布在车厢1两侧的安装凸台上，外筒17为空心的，A端开口，且有另一端面B；内筒15的端面A分别可移动地内套于对应的外筒17内，内筒15的另一端面B与后万向轮14焊接在一起并从外筒17的A端伸出；内筒15的端面A与外筒17的端面B之间通过弹簧16连接。

如图1所示，控制***包括ARM嵌入式控制器2、左右编码器11、3，左右伺服电机驱动器9、5，无线数传模块12，信号调理与变送模块8和温度检测模块13。ARM嵌入式控制器2为自主移动车辆控制核心，提供及时信息处理和与上位机的通信。左编码器11和右编码器3外壳分别与左直流伺服电机10和右直流伺服电机4外壳固定，其转子与对应的伺服电机转子采用柔性连接器相互连接，用于实时检测左右伺服电机10、4的速度。固定在车厢1内的ARM嵌入式控制器2经RS485通信总线以串行方式与左伺服电机驱动器9和右伺服电机驱动器5相连，左伺服电机驱动器9引出两条导线，一条与左直流伺服电机10相连，一条与左编码器11相连；右伺服电机驱动器5引出两条导线，一条与右直流伺服电机4相连，一条与右编码器3相连。左右伺服电机驱动器9、5通过RS485通信总线以串行方式与ARM嵌入式控制器2连接。考虑到温室内存在的各种干扰，所设的无线数传模块12与ARM嵌入式控制器2相连，实现与上位机的无线串行数字通信功能。六个信号调理与变送模块8与超声波测距传感器A71、超声波测距传感器B72、超声波测距传感器C73、超声波测距传感器D74、超声波测距传感器E75和超声波测距传感器F76一一相连，并通过RS232的通信总线与ARM嵌入式控制器2连接。所设温度检测模块13与ARM嵌入式控制器2连接，实现对温室内温度实时检测。

如图1，图4，图5所示，六个超声波测距传感器7包括安装在车厢1的前后两测量点处的超声波测距传感器C73、超声波测距传感器F76，安装在左侧两测量点处的超声波测距传感器D74、超声波测距传感器E75和安装在右侧的两测量点处的超声波测距传感器A71、B72。超声波测距传感器D74、超声波测距传感器E75和超声波测距传感器A71、超声波测距传感器B72关于车厢1纵向中心面对称，且超声波测距传感器A71、超声波测距传感器E75和超声波测距传感器B72、超声波测距传感器D74关于车厢1横向中心面对称，左右两侧的每对超声波安装距离为3/4的车身长度。前后两侧的超声波测距传感器C73、超声波测距传感器F76位于车厢纵向中心面上。每个超声波传感器包括超声波发射器，超声波接收器，超声波驱动电路与超声波接收电路组成。在自主移动车辆开始工作时，固定在车厢内部的蓄电池组6通过导线给各部件提供电源，超声波驱动电路发出经过滤波、整形，放大后的方波信号，超声波发射器发射指定频率的超声波，经过放射后的超声波经超声波接收器接收并经超声波接收电路处理传送到对应的信号调理与变送模块8，由信号调理与变送模块直接传送到ARM嵌入式控制器2。由于温室内温度与室外温度的差异，所设温度检测模块，实时检测温室内温度传送给ARM嵌入式控制器，ARM嵌入式控制器2计算来自超声波测距传感器A71、超声波测距传感器B72、超声波测距传感器C73、超声波测距传感器D74、超声波测距传感器E75、超声波测距传感器F76信息，得出当前自主移动车辆航向偏差与偏角，再由RS485通信总线控制左伺服电机驱动器9、右伺服电机驱动器5来控制相应的电机速度。

如图5、图7、图8、图9、图10所示，自主移动车辆在温室垄间往返行走时，主要通过相应的超声波测距传感器计算出当前车辆的位置信息，以一次在江苏大学连栋温室所测数据为例(如图5)，该温室垄宽为65cm，自主移动车辆车宽为45cm，车长为1m，当时检测连栋温室内的温度为25⁰，室外温度为20⁰，自主移动车辆以1m/s的匀速前进，理想的行走路径为每行垄两侧墙的平分线，具体实施方法如下：

步骤1:上位机通过无线数传模块向自主移动车辆发射启动请求信号，移动车辆接收到请求信号后，控制器初始化，启动驱动程序，设置导航***总定时器时间为T₁＝0.1s，即每隔0.1s***向四周发射超声波，并直接启动驱动程序，驱动电机带动车辆前进。

步骤2：自主移动车辆向前移动时，总定时器从零计时，到达0.1s时，进入定时中断子程序，设置所有超声波发射装置的初始时间为t₀＝0s，控制器经驱动电路控制超声波测距传感器A71、超声波测距传感器B72、超声波测距传感器C73、超声波测距传感器D74、超声波测距传感器E75和超声波测距传感器F76发射超声波，所有超声波发射装置发射完成后，停止发射，返回程序。

步骤3：当超声波测距传感器A71、超声波测距传感器B72、超声波测距传感器C73、超声波测距传感器D74、超声波测距传感器E75和超声波测距传感器F76开始接收到回波时，此处设等待接收超声波时间为T₂＝0.02s，等待时间结束后，启动外部中断子程序，关闭所有的超声波发射装置中断定时器，读取时间t₁＝0.706ms、t₂＝0.238ms、t₃＝5.33ms、t₄＝0.107ms、t₅＝0.287ms，t₆＝0.02s，这样超声波测距传感器A71、超声波测距传感器B72、超声波测距传感器C73、超声波测距传感器D74、超声波测距传感器E75、超声波测距传感器F76从发射超声波到接收超声波的时间分别为(t₁-t₀)＝0.706ms、(t₂-t₀)＝0.238ms、(t₃-t₀)＝5.33ms、(t₄-t₀)＝0.107ms、(t₅-t₀)＝0.287ms、(t₆-t₀)＝0.02s。为了补偿温室温度对超声波在空气中速度的影响，所安装的温度检测模块能实时的检测出当前的温室的温度T＝25⁰，则超声波速度在温室中的传播速度为：

$V=331.4\sqrt{1+\frac{T}{273}}=375.24m/s$

步骤4：计算自主移动车辆各超声波测距传感器安装的测量点处与对应垄侧墙壁的距离为：

D1＝V×(t₁-t₀)/2＝13.246cm

D2＝V×(t₂-t₀)/2＝4.465cm

D3＝V×(t₃-t₀)/2＝1m

D4＝V×(t₄-t₀)/2＝2.007cm

D5＝V×(t₅-t₀)/2＝10.769cm

D6＝V×(t₆-t₀)/2＝3.752m

步骤5：计算自主移动车辆当前的航向偏差ΔX与航向偏角Δθ：

$\mathrm{\α}=\arctan \frac{D4-D3}{D0}=4.952^0$

$\mathrm{\β}=\arctan \frac{D1-D2}{D0}{=5.023}^0$

$\mathrm{\Δ\θ}=\frac{\mathrm{\α}+\mathrm{\β}}{2}=4.988^0,\mathrm{\δ}=\mathrm{\Δ\θ}=4.9{88}^0$

$\mathrm{\ΔX}=\frac{|(D3+D4)/2|-|(D1+D2)/2|}{2}\cos \mathrm{\Δ\θ}=-1.254\mathrm{cm}$

步骤6：根据该连栋温室的实际情况，自主移动车辆与前后障碍物所设定的最小安全距离为80cm，即D5_{min_distance}＝D6_{min_distance}＝80cm，计算可得D3cosδ＝0.996m，D6cosδ＝3.738m，此时D3cosδ>D3_{min_distance}且D6cosδ>D6_{min_distance}，可以判定当前自主移动车辆前后无障碍，由于Δθ≠0或ΔX≠0，故ARM嵌入式控制器通过控制左右伺服电机驱动器调整左右伺服电机速度，以消除前进的航向偏差与偏角。以自主移动车辆前进方向为基准，且定义Δθ＞0时，移动车辆当前方向是向右偏的，移动车辆上的ARM控制器控制左右伺服电机速度进行左转向调整，Δθ＜0时，与Δθ＞0时情况相反；以自主移动车辆前进方向为基准，且定义ΔX＞0时，小车重心与右垄靠近，移动车辆需要左移调整，ΔX＜0时，与ΔX＞0的情况相反。此时由于Δθ＞0且ΔX＜0，自主移动车辆上的ARM控制器控制左伺服电机速度大于右伺服电机速度并进行左转向调整。

步骤7：自主移动车辆自身调整完成后，将相关运行状态经无线数传模块反馈到上位机，此时自主移动车辆总定时时间设定为0，重新进入步骤1。

自主移动车辆实施转向时，如图6所示，自主移动车辆在温室垄端实施转向时，自动向上位机发送请求，上位机通过无线数传模块向其发射原地转向指令。在①处的温室移动车辆通过调整左右伺服电机的速度实施顺时针原地转向90⁰,行进到②处时，通过位于移动车辆的一侧的一对超声波测距传感器发射超声波检测的距离此时检测的距离为D6＝9.56cm，D7＝10.87cm，则：

$\mathrm{\λ}=\arctan \frac{D7-D6}{D0}=1.0{21}^0$

通过ARM嵌入式控制器通过调整左右伺服电机的速度使自主移动车辆向右偏转即可消除偏角。自主移动车辆行走位于③处时，上位机通过无线数传模块向自主移动车辆发射原地转向指令，在①处的温室移动车辆通过调整左右伺服电机的速度实施顺时针原地转向90⁰，此时温室移动车辆进入准备进入下一垄实施工作。

Claims (1)

1.一种应用于温室的自主移动车辆超声波导航方法，其特征在于包括以下步骤：

步骤1，上位机通过无线数传模块向自主移动车辆发射启动请求信号，移动车辆接收到请求信号后，ARM嵌入式控制器初始化，启动驱动程序，设置导航***总定时器时间为T₁，使每隔时间T₁自主移动车辆向四周发射超声波，考虑到自主移动车辆与左右垄侧墙壁的距离狭窄，设定T₁在0.1s—0.5s之间，此时直接启动驱动程序，驱动电机运动带动车辆前进；

步骤2，移动车辆向前移动时，总定时器从零计时，到达时间T₁时，进入定时中断子程序，设置所有超声波发射装置的初始时间为t₀，ARM嵌入式控制器经驱动电路控制超声波测距传感器(7)发射超声波，所有超声波发射装置发射完成后，停止发射，返回程序；

步骤3，当超声波测距传感器A(71)、超声波测距传感器B(72)、超声波测距传感器C(73)、超声波测距传感器D(74)、超声波测距传感器E(75)和超声波测距传感器F(76)开始接收到回波时，此处设等待接收超声波时间为T₂，考虑到超声波传播速度与狭窄垄间的距离，设定T₂在0.02s—0.08s之间，在自主移动车辆的前后方无障碍时，超声波测距传感器C(73)和超声波测距传感器F(76)检测不到回波时，读取时间自动赋值为T₂，等待时间结束后，启动外部中断子程序，关闭所有的超声波发射装置中断定时器；读取超声波测距传感器A(71)、超声波测距传感器B(72)、超声波测距传感器C(73)、超声波测距传感器D(74)、超声波测距传感器E(75)和超声波测距传感器F(76)接收到超声波回波后的记录时间分别为t₁、t₂、t₃、t₄、t₅、t₆，6个超声波测距传感器从发射超声波到接收超声波的时间依次分别为(t₁-t₀)、(t₂-t₀)、(t₃-t₀)、(t₄-t₀)、(t₅-t₀)、(t₆-t₀)；

为补偿温室温度对超声波在空气中速度的影响，所安装的温度检测模块能实时的检测出当前的温室的温度T，则超声波速度在温室中的传播速度为：

$V=331.4\sqrt{1+\frac{T}{273}}$

步骤4，计算自主移动车辆各超声波测距传感器安装的测量点处与对应垄侧墙壁的距离为：

D1＝V×(t₁-t₀)/2

D2＝V×(t₂-t₀)/2

D3＝V×(t₃-t₀)/2

D4＝V×(t₄-t₀)/2

D5＝V×(t₅-t₀)/2

D6＝V×(t₆-t₀)/2

步骤5，计算自主移动车辆当前的航向偏差ΔX与航向偏角Δθ：

$\mathrm{\α}=\arctan \frac{D4-D3}{D0}$

$\mathrm{\β}=\arctan \frac{D1-D2}{D0}$

$\mathrm{\Δ\θ}=\frac{\mathrm{\α}+\mathrm{\β}}{2},\mathrm{\δ}=\mathrm{\Δ\θ}$

$\mathrm{\ΔX}=\frac{|(D3+D4)/2|-|(D1+D2)/2|}{2}\cos \mathrm{\Δ\θ}$

步骤6，在自主移动车辆前超声波测距传感器所测自主移动车辆前测量点与障碍的实际距离大于等于自主移动车辆前测量点与障碍物最小安全距离且后超声波测距传感器所测自主移动车辆后测量点与障碍的实际距离大于等于自主移动车辆后测量点与障碍物最小安全距离情况下，当Δθ＝0且ΔX＝0时，继续运行左右伺服电机，无需角度调整；而当Δθ≠0或ΔX≠0时，ARM嵌入式控制器通过左右伺服电机驱动器调整左右伺服电机速度，以消除前进的航向偏差与偏角，以自主移动车辆前进方向为基准，且定义Δθ＞0时，移动车辆当前方向是向右偏的，移动车辆上的ARM控制器控制左右伺服电机速度进行左转向调整，Δθ＜0时，与Δθ＞0时情况相反；以自主移动车辆前进方向为基准，且定义ΔX＞0时，自主移动车辆重心与右垄靠近，移动车辆需要左移调整，ΔX＜0时，与ΔX＞0的情况相反；在自主移动车辆前超声波测距传感器所测自主移动车辆前测量点与障碍的实际距离小于自主移动车辆前测量点与障碍物最小安全距离或后超声波测距传感器所测自主移动车辆后测量点与障碍的实际距离大于等于自主移动车辆后测量点与障碍物最小安全距离情况下，ARM嵌入式控制器控制驱动电机停止转动，自主移动车辆垄间准备后退时，在自主移动车辆后退的过程中，超声波检测的方法与自主移动车辆前进时相同；

步骤7，自主移动车辆自身调整完成后，将相关运行状态经无线数传模块反馈到上位机，此时自主移动车辆总定时时间设定为0，返回步骤1。