CN109489666B - 温室喷药机器人同步定位与地图构建的方法 - Google Patents

Abstract

本发明公开了一种温室喷药机器人同步定位与地图构建的方法，在未知环境中，温室喷药机器人首先通过运动模型预测出自身的位姿，然后通过传感器得到自身与路标的相对信息，再由观测模型估计出路标的位置并更新温室喷药机器人位姿。通过采用改进的协同算法，对导航误差进行控制并构建准确的环境模型，这样可以有助于实现高精度定位，同时，温室喷药机器人在移动的过程中可以构建一个温室环境地图，同时使用这个地图推测它自身的位置，从而实现温室喷药机器人的同步定位与地图构建。

Description

温室喷药机器人同步定位与地图构建的方法

技术领域

本发明涉及机器人定位领域，具体涉及一种温室喷药机器人同步定位与地图构建的方法。

背景技术

温室喷药机器人是一类通过传感器感知周围环境和自身状态，实现在有障碍物的工作环境中向目标移动的自主运动，从而完成喷药任务的机器人。通过温室喷药机器人携带的里程计、陀螺仪、视觉、激光雷达等传感器进行状态及环境感知来解决定位问题。定位指温室喷药机器人必须通过内、外部传感器确定自身在所工作环境中的精确位置。该工作环境在温室喷药机器人内部呈现出环境模型(或地图)，即温室喷药机器人所工作环境中的各种物体(如障碍、路标等)准确的空间位置、结构等描述。准确的环境模型有助于实现高精度定位，而精确定位则有利于建立更可信的环境模型。

发明内容

本发明的目的在于提供一种温室喷药机器人同步定位与地图构建的方法，对温室喷药机器人进行路径规划及避障设计,充分利用现有传感器,结合多传感器融合技术,实现温室喷药机器人的路径规划与地图构建。

为实现上述目的，本发明采取的技术方案为：

温室喷药机器人同步定位与地图构建的方法，包括以下步骤：

S1、温室喷药机器人在由运动模型预测自己的位姿时，根据传感器得到温室喷药机器人与路标之间的相对信息，再由逆观测模型推测出路标的位置；

S2、基于策略协同的异构型协同进化算法(SSHCEA)，通过种群合作可以扬长避短，快速找到区域内要搜索的最优解；

S3、将SSHCEA算法用于对导航误差进行控制并构建准确的环境模型，实现高精度定位，同时，温室喷药机器人在移动的过程中可以构建一个温室环境地图，并通过这个地图推测它自身的位置，实现温室喷药机器人的同步定位与地图构建，从而完成喷药任务。

进一步地，所述温室喷药机器人运动模型如下：

其中,d是温室喷药机器人的小车在单位时间内的移动距离(温室喷药机器人的小车速度×控制信号时间)；α为控制信号的角度；x_v，y_v，θ_v分别为温室喷药机器人的小车在平面地图中的横坐标、纵坐标和航向角。

进一步地，所述观测模型由温室喷药机器人根据当前时刻自身位姿和路标位置得出的观测信息，观测模型如下：

其中，d为超声波传感器的距离信息，

为水平方向的云台的角度，x_v，y_v，θ_v分别为温室喷药机器人的小车在平面地图中的横坐标、纵坐标和航向角，a为超声波位置到温室喷药机器人的小车前轮中心轴的距离，L为温室喷药机器人的小车前轮中心轴到后轮中心轴的距离。

进一步地，所述步骤S2具体包括如下步骤：

S21、算法初始化：

假设，在k-1时刻，联合状态的表达式为

完成种群的区域规模，粒子维数，迭代次数的初始化；对于种群中的每个粒子，根据其历史信息，计算出一个区域分布，并从分布中进行采样，再将这种新的样本加入到粒子的历史中；

在k时刻，各变量做以下定义：

x_k：表示温室喷药机器人的位置和航向角；

u_k：控制矢量，表示温室喷药机器人从状态x_k-1到x_k的控制信号；

m_i：是一个向量，表示第i个路标的真实位置；

z_i,k：表示在k时刻温室喷药机器人观测到第i个路标。将z_i,k简化表示为z_k；

X_0:k＝{x₀,x₁,…,x_k}＝{X_0:k-1,x_k}，为温室喷药机器人的历史位置信息；

U_0:k＝{u₀,u₁,…,u_k}＝{U_0:k-1,u_k}，为控制输入的历史信息；

m＝{m₁,m₂,…,m_n}，为所有路标的集合；

Z_0:k＝{z₁,z₂,…,z_k}＝{Z_0:k-1,z_k}，为所有路标的观测信息；

给定轨迹的粒子，时间从0时刻到K时刻，i代表第i个粒子。

在k时刻，联合概率分布是有一组

表示，其中伴随着每个粒子的地图时由独立的高斯分布

表示；

SLAM联合状态是由温室喷药机器人位姿和一个有条件的地图构成：

这里，概率分布是在基于轨迹X_0:k的分布，而不是单一的温室喷药机器人位姿x_k，这是因为基于轨迹的条件时，在地图上的路标就是独立的；在k时刻，联合概率分布是有一组

表示，其中伴随着每个粒子的地图时由独立的高斯分布

表示；

S22、随机产生1个初始向量Z₁＝(Z₁₁,Z₁₂,...,Z_1D),Z_1i＝[0,1]，其中，D为目标函数的维数；根据Logistic映射式得到N个向量Z₁,Z₂,...,Z_N，将其变换到目标函数的初始值取值区间X₁,X₂,...,X_N，由此便实现了一种群的混沌序列初始化，另一种群采用随机生成策略生成一组初始化参数位置信息；

S23、根据重要性函数计算权重

S24、两种群同时采样，选择与

成比例的概率的一些粒子，对集合

中的粒子，替换与他们相关的地图信息。选中粒子的权重为

S25、对于每种群中的每个粒子，当温室喷药机器人位姿已知时，对所观察到的路标进行更新。

上述方案中，在未知环境中，温室喷药机器人能够通过传感器得到自身与路标的相对信息，再通过运动模型预测出温室喷药机器人位姿。同时，再由观测模型估计出路标的位置并更新温室喷药机器人位姿。通过采用改进的协同算法，对导航误差进行控制并构建准确的环境模型，这样可以有助于实现高精度定位，同时，温室喷药机器人在移动的过程中可以构建一个温室环境地图，同时使用这个地图推测它自身的位置，从而实现温室喷药机器人的同步定位与地图构建。

附图说明

图1为本发明实施例中的温室喷药机器人的小车结构示意图。

图2为为本发明实施例中的温室喷药机器人的小车的观测模型示意图。

图3为本发明实施例中的温室喷药机器人运动模型图。

图4为本发明实施中的温室喷药机器人的小车系统结构图。

图5为本发明实施例中单个粒子实现温室喷药机器人运动轨迹的过程.

图6为本发明实施例中的实验环境地图示意图。

具体实施方式

为了使本发明的目的及优点更加清楚明白，以下结合实施例对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

本发明实施例的温室喷药机器人同步定位与地图构建的方法，包括以下步骤：

S1、温室喷药机器人首先由运动模型预测自己的位姿，根据传感器得到温室喷药机器人与路标之间的相对信息，再由观测模型推测出路标的位置；

温室喷药机器人静态环境下，因路标位置不随时间改变，因此系统状态转移只需考虑自身运动状态的变化。运动模型的控制输入量u包含机器人在行驶中的旋转角度α和运动距离d，温室喷药机器人的运动行程是由旋转角度或旋转后直行距离所构成。

如图1所示，温室喷药机器人运动模型如下：

其中，d是温室喷药机器人的小车在单位时间内的移动距离(温室喷药机器人的小车速度×控制信号时间)，α为控制信号的角度。x_v,y_v,θ_v分别为温室喷药机器人的小车在平面地图中的横坐标、纵坐标和航向角。

温室喷药机器人从初始位置A开始直行d₁距离到达位置B，此过程中温室喷药机器人的前进方向不发生改变。为了从位置B到达位置C，温室喷药机器人的运动过程分为两个步骤。温室喷药机器人到达B点后逆时针旋转α角，然后直行d₂距离到达C点。

由图3得到的温室喷药机器人运动模型如下：

观测模型由温室喷药机器人根据当前时刻自身位姿和路标位置得出的观测信息。如图2所示，观测模型如下：

其中，d为超声波传感器的距离信息，

为水平方向的云台的角度。

S2、基于策略协同的异构型协同进化算法(SSHCEA)，通过种群合作可以扬长避短，快速找到区域内要搜索的最优解；

S21、算法初始化：

假设，在k-1时刻，联合状态的表达式为

完成种群的区域规模，粒子维数，迭代次数的初始化；对于种群中的每个粒子，根据其历史信息，计算出一个区域分布，并从分布中进行采样，再将这种新的样本加入到粒子的历史中；

在k时刻，各变量做以下定义：

x_k：表示温室喷药机器人的位置和航向角；

u_k：控制矢量，表示温室喷药机器人从状态x_k-1到x_k的控制信号；

m_i：是一个向量，表示第i个路标的真实位置；

z_i,k：表示在k时刻温室喷药机器人观测到第i个路标。将z_i,k简化表示为z_k；

X_0:k＝{x₀,x₁,…,x_k}＝{X_0:k-1,x_k}，为温室喷药机器人的历史位置信息；

U_0:k＝{u₀,u₁,…,u_k}＝{U_0:k-1,u_k}，为控制输入的历史信息；

m＝{m₁,m₂,…,m_n}，为所有路标的集合；

Z_0:k＝{z₁,z₂,…,z_k}＝{Z_0:k-1,z_k}，为所有路标的观测信息；

给定轨迹的粒子，时间从0时刻到K时刻，i代表第i个粒子。

在k时刻，联合概率分布是有一组

表示，其中伴随着每个粒子的地图时由独立的高斯分布

表示；

SLAM联合状态是由温室喷药机器人位姿和一个有条件的地图构成：

这里，概率分布是在基于轨迹X_0:k的分布，而不是单一的温室喷药机器人位姿x_k，这是因为基于轨迹的条件时，在地图上的路标就是独立的；在k时刻，联合概率分布是有一组

表示，其中伴随着每个粒子的地图时由独立的高斯分布

表示；

S22、随机产生1个初始向量Z₁＝(Z₁₁,Z₁₂,...,Z_1D),Z_1i＝[0,1]，其中，D为目标函数的维数；根据Logistic映射式得到N个向量Z₁,Z₂,...,Z_N，将其变换到目标函数的初始值取值区间X₁,X₂,...,X_N，由此便实现了一种群的混沌序列初始化。另一种群采用随机生成策略生成一组初始化参数位置信息；

S23、根据重要性函数计算权重

S24、两种群同时采样，选择与

成比例的概率的一些粒子，对集合

中的粒子，替换与他们相关的地图信息。选中粒子的权重为

S25、对于每种群中的每个粒子，当温室喷药机器人位姿已知时，对所观察到的路标进行更新。

S3、将SSHCEA算法用于对导航误差进行控制并构建准确的环境模型，实现高精度定位，同时，温室喷药机器人在移动的过程中可以构建一个温室环境地图，并通过这个地图推测它自身的位置，实现温室喷药机器人的同步定位与地图构建，从而完成喷药任务。

实施例

本实施例中的温室喷药机器人的总体结构框图如图2所示,该温室喷药机器人包括arduino开源硬件+扩展板,电机驱动板,带两个独立云台的超声波传感器,MPU-6050模块(三轴陀螺仪+三轴加速度),指南针,两个光电编码器,,USR-WIFI232-T通讯模块,四个直流电机,6节1.2v2600mA镍氢充电电池。系统以arduino-mega2650作为控制核心,外接四个独立直流电机驱动板,控制温室喷药机器人的小车的四个驱动轮。同时控制超声波传感器的两个独立云台及对超声波传感器信号的发送与接收,读取三轴陀螺仪、三轴加速度、两个光电编码器的信号,并通过UsR-wIFI232-T通讯模块实现温室喷药机器人与个人电脑的无线通讯,数据交换。

1)控制方面

温室喷药机器人的小车的控制核心采用Arduino-mega2650开源硬件,为了使用方便,我们外接了Anluino扩展板,它将全部数字与模拟接口以舵机线序形式扩展出来,还特设蓝牙模块通信接口、SD卡模块通信接口、APC220通讯模块、ICSP按口等。

2)驱动模块

直流驱动模块采用L298N驱型模块,采用SMT工艺稳定性高集成LM2596DC_DC稳压芯片,可以输出5v给单片机,舵机,传感器,路由器等,电池最大可达2A。

3)测速模块

ZK_measure测速模块使用一对射红外发射接收管WYC H206,74HC14(带施密特触发器反相器),防止l111界输出抖动作为核心器件构成中控电路。

4)三轴陀螺仪+三轴加速度

MPU-6050模块(三軸陀螺仪+三轴加速度)整合了3轴陀螺仪、3轴加速度器,以数字输出或9轴的旋转矩阵、四元数(quatemion)、欧拉角格式(Euler angleform)的融合验算数据。

5)串口联网模块

USR-WIFI232-T用于实现串口到WIFI数据包的双向透明转发,模块内部完成协议转换,串口一側数据透明传输,Wnl1网络一例是TCPIIP数据包,通过该模组,传统的低端串口设各或MCU控制的设各可以方便的接入WIFI无线网络,从而实現物联网络控制和管理,在这里我们通过该棋块可以实現温室喷药机器人的小车和Pc机的无线通讯,进行数据的接收和发送。

6)传感器模块

选择Hc-sR04超声波测距模块，它可提供2cm-400cm的非接触式距离感测功能,测距精度可高达3mm；模块包括超声波发射器、接收器与控制电路。

基本工作原理：

1、采用IO口TRIG出发测距，给至少10us的高电平信号；

2、模块自动发送8个40khz的方波，自动检测是否有信号返回；

3、有信号返回，通过IO口ECHO输出一个高电平，高电平持续的时间就是超声波从发射到返回的时间。测试距离＝(高电平时间*声速(340M/S)/2)。

本发明实施例中，采用的是Arduino-mega2650作为控制板，在Arduino1.0.5环境中编写温室喷药机器人的小车的控制程序，数据传输，定义云台、超声波传感器、陀螺仪、测速模块的地址等，编译好程序之后可以通过ISP在线烧写程序到Arduino里。在这个温室喷药机器人的小车控制程序中，让温室喷药机器人的小车绕着搭建的“回”字形环境自主行走，并有一定的避障功能。在运动的过程中将传感器信息通过WIFI传送到PC机，以便进行后期的数据处理。

对于Arduino与PC机或者服务器的数据通讯，采用了最常见的WIFI无线通讯模式，遵从TCP/IP通讯协议，从而更好的应用于我们的生活中。为了实现它们之间的通讯，需要编写SERVER端和CLIENT端的SOCKET程序，下面是基于visual studio 2010编写的相应程序。

图5为本发明实施例中单个粒子实现温室喷药机器人运动轨迹的过程，图中椭圆显示每个更新阶段的建议分布从一个温室喷药机器人的位姿进行采样，并假设这个位姿时完美的，那么观测到的路标同时被更新。因此，地图中路标的精确度是受温室喷药机器人运动轨迹准确度的支配的。许多这样的轨迹提供了温室喷药机器人的位置的概率模型。

图6为本发明实施例中实验地图的示意图，温室喷药机器人的小车起始位置为地图的原点，内框线和外框线是拟合实验环境的边界线，内框线和外框线四个角上的五角星是根据传感器等信息构成的特征点的位置,其余五角星是将八个边角作为特征点带入到算法中得到路标的估计位置。

以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以作出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims (3)

1.温室喷药机器人同步定位与地图构建的方法，其特征在于：包括以下步骤：

S1、温室喷药机器人首先由运动模型预测自己的位姿，根据传感器得到温室喷药机器人与路标之间的相对信息，再由观测模型推测出路标的位置；

S2、基于策略协同的异构型协同进化算法SSHCEA，通过种群合作扬长避短，快速找到区域内要搜索的最优解；

具体包括如下步骤：

S21、算法初始化：

假设，在k-1时刻，联合状态的表达式为

完成种群的区域规模，粒子维数，迭代次数的初始化；对于种群中的每个粒子，根据其历史信息，计算出一个区域分布，并从分布中进行采样，再将这种新的样本加入到粒子的历史中；

在k时刻，各变量做以下定义：

x_k：表示温室喷药机器人的位置和航向角；

u_k：控制矢量，表示温室喷药机器人从状态x_k-1到x_k的控制信号；

m_i：是一个向量，表示第i个路标的真实位置；

z_i,k：表示在k时刻温室喷药机器人观测到第i个路标，将z_i,k简化表示为z_k；

X_0:k＝{x₀,x₁,…,x_k}＝{X_0:k-1,x_k}，为温室喷药机器人的历史位置信息；

U_0:k＝{u₀,u₁,…,u_k}＝{U_0:k-1,u_k}，为控制输入的历史信息；

m＝{m₁,m₂,…,m_n}，为所有路标的集合；

Z_0:k＝{z₁,z₂,…,z_k}＝{Z_0:k-1,z_k}，为所有路标的观测信息；

给定轨迹的粒子，时间从0时刻到K时刻，i代表第i个粒子，

在k时刻，联合概率分布是有一组

表示，其中伴随着每个粒子的地图时由独立的高斯分布

表示；

SLAM联合状态是由温室喷药机器人位姿和一个有条件的地图构成：

这里，概率分布是在基于轨迹X_0:k的分布，而不是单一的温室喷药机器人位姿x_k，这是因为基于轨迹的条件时，在地图上的路标就是独立的；

S22、随机产生1个初始向量Z₁＝(Z₁₁,Z₁₂,...,Z_1D),Z_1i＝[0,1]，其中，D为目标函数的维数；根据Logistic映射式得到N个向量Z₁,Z₂,...,Z_N，将其变换到目标函数的初始值取值区间X₁,X₂,...,X_N，由此便实现了一种群的混沌序列初始化，另一种群采用随机生成策略生成一组初始化参数位置信息；

S23、根据重要性函数计算权重

在每一个时间步k，粒子从接近于真实分布P(x_k|X_0:k-1,Z_0:T)的建议分布π(x_k|X_0:k-1,Z_0:k)中采样得到，同时通过对粒子的加权弥补任何两者之间的差异；

S24、两种群同时采样，选择与

成比例的概率的一些粒子，对集合

中的粒子，替换与他们相关的地图信息，选中粒子的权重为

S25、对于每种群中的每个粒子，当温室喷药机器人位姿已知时，对所观察到的路标进行更新；

S3、将SSHCEA算法用于对导航误差进行控制并构建准确的环境模型，实现高精度定位，同时，温室喷药机器人在移动的过程中构建一个温室环境地图，并通过这个地图推测它自身的位置，实现温室喷药机器人的同步定位与地图构建，从而完成喷药任务。

2.如权利要求1所述的温室喷药机器人同步定位与地图构建的方法，其特征在于：所述温室喷药机器人运动模型如下：

其中,κ表示时间，d是温室喷药机器人的小车在单位时间内的移动距离；α为控制信号的角度；x_v，y_v，θ_v分别为温室喷药机器人的小车在平面地图中的横坐标、纵坐标和航向角。

3.如权利要求1所述的温室喷药机器人同步定位与地图构建的方法，其特征在于：所述观测模型由温室喷药机器人根据当前时刻自身位姿和路标位置得出的观测信息，观测模型如下：

其中，d为超声波传感器的距离信息，

为水平方向的云台的角度，x_v，y_v，θ_v分别为温室喷药机器人的小车在平面地图中的横坐标、纵坐标和航向角；a为超声波位置到温室喷药机器人的小车前轮中心轴的距离，L为温室喷药机器人的小车前轮中心轴到后轮中心轴的距离。

Images