用于自动引导车辆的近场低频无线定位方法
技术领域
本发明属于自动化技术领域,特别是涉及一种用于自动引导车辆的近场低频无线定位方法。
背景技术
自动引导车辆在结构化道路环境中自动行驶时,路径的跟踪是以预先设定的固定引导路径为基础来控制运动方向。为了实现在非道路环境下车辆的自动引导,可以采用无线射频定位引导技术,这一工作由无线定位模块完成。自动引导车辆的状态控制以无线定位模块为基础,根据模块的结果,控制自动引导车辆完成车体的加速、减速、转向和停车等动作。因此,无线定位模块的速度和精度是保证自动引导车辆实时、准确和可靠运动的关键。
无线定位技术有很多种,这里采用基于RSSI(Received Signal StrengthIndicator,接收信号强度指示)的低频无线定位技术,它是一种定位精度高、功耗低、成本低、反应速度快的高性价比无线定位技术。在基于RSSI的低频定位中,已知低频发射基站的发射信号强度,待定位模块(以下称为引导模块)根据实际接收到信号强度计算出信号的传播损耗,利用理论和经验模型将传输损耗转化为距离,再利用一定的算法计算出引导模块的位置,位置信息包括方位角和直线距离。
无线定位模块设计过程中需要克服以下问题:1、克服外界环境变化对定位结果的影响;2、克服随机干扰对定位精度的影响;3、克服天线方向性对定位结果的影响;4、满足系统大负荷的运算。
发明内容
本发明的目的是针对现有技术的不足,提出了一种用于自动引导车辆的近场低频无线定位方法。
本发明方法的具体步骤是:
步骤(1)对车载定位模块的控制器初始化,初始化对象包括控制器中的单片机时钟频率、输入输出、内部变量、寄存器、非易失性存储器和中断;
步骤(2)车载定位模块的控制器从非易失性存储器中读取场地环境对应的标定参数,所述的标定参数包括路径衰减指数λ、标准偏差为σ的正态随机变量ξσ、d0米处引导模块收到的RSSI信号强度值RSSI(d0);λ、ξσ和RSSI(d0)采用最小二乘法根据场地环境拟合得到;
步骤(3)车载定位模块等待来自电机控制模块的定位启动命令;如果收到电机控制模块的定位启动命令,则执行步骤(4),如果未收到电机控制模块的定位启动命令,则重复本步骤;
步骤(4)车载定位模块通过自带的高频通信模块,在433.05MHz~434.79MHz频段内向引导模块发送初始化命令,使能引导模块初始化;
引导模块初始化其内部的控制器中的单片机时钟频率、输入输出、内部变量、寄存器和中断,使能引导模块上的三维低频接收器;
步骤(5)车载定位模块关闭所有N个低频发射基站的低频发射功能,使能第n个低频发射基站的低频发射功能,发射频率恒定为fc,发射RSSI信号强度值恒定为RSSI(C),同时车载定位模块通过自带的高频通信模块,在433.05MHz~434.79MHz频段内向引导模块发送接收命令,其中n为自然数,且n≤N;
引导模块接收来自第n个低频发射基站的低频无线电信号,得到接收RSSI信号强度值RSSI(dn);引导模块自带的高频通信模块,在433.05MHz~434.79MHz频段内向车载定位模块发回RSSI(dn),车载定位模块记录该值并保存于内存中;
车载定位模块计算引导模块距第n个低频发射基站的距离d
n,
步骤(6)延时设定的时间t,重复执行步骤(5)直到得到N个低频发射基站中每个低频发射基站对应的距离d1、d2、d3...dN-1、dN;
以任意一个低频发射基站为原点,建立空间三维坐标系,则d1、d2、d3...dN-1、dN分别对应N个低频发射基站三维坐标(x1,y1,z1)、(x2,y2,z2)、(x3,y3,z3)…(xN,yN,zN);
步骤(7)在d1、d2、d3...dN-1、dN中任选3个距离值,分别标记为dm1、dm2、dm3,其对应的三个低频发射基站三维坐标记为(xm1,ym1,zm1)、(xm2,ym2,zm2)、(xm3,ym3,zm3);
解方程组
得到一个定位出的引导模块的三维坐标值(x
m,y
m,z
m);
步骤(8)重复执行步骤(7)
次,得到
个引导模块的三维坐标值
步骤(9)根据步骤(8)得到的
个引导模块的三维坐标值,计算引导模块的最终三维坐标值(x
r,y
r,z
r);
步骤(10)车载定位模块向电机控制模块发送最终三维坐标值(xr,yr,zr);
步骤(11)重复步骤(5)至步骤(10),实时计算出引导模块的三维坐标,以使自动引导车辆跟随引导模块运行,直至车载定位模块的电源被关闭为止。
本发明所具有的有益效果:
(1)、自适应能力强,能较好克服外界环境对定位算法的影响;
(2)、无线定位结果精度高、反应速度快;
(3)、能较好克服天线方向性对定位结果的影响,能满足系统大负荷运算。
附图说明
图1为本发明使用场合的结构示意图。
具体实施方式
以下结合附图对本发明作进一步说明。
如图1所示,车载定位模块1上装有第一低频发射基站1-1-1、第二低频发射基站1-1-2、第三低频发射基站1-1-3、…、第N低频发射基站1-1-N,共计N个低频发射基站和一个高频通信模块1-2,低频发射基站的发射功能和高频通信模块的通信功能均由车载定位模块1上的控制器控制。在一定距离处的引导模块2上装有三维低频接收器2-1,该接收器能有效地接受来自低频发射基站的低频无线电信号,引导模块上的高频通信模块2-2与车载定位模块上的高频通信模块2-1在一定的频域内进行通信。
在上述基础上,本发明方法的具体步骤如下:
步骤(1)对车载定位模块的控制器1-3初始化,初始化对象包括控制器中的单片机时钟频率、输入输出、内部变量、寄存器、非易失性存储器和中断;车载定位模块的控制器采用Freescale公司的16位双核微控制器MC9S12XEP100,该控制器内部包含一个频率可达100MHz的协处理器,可以用来完成大量的数学运算,内部含有非易失性存储器,使用中采用的CPU线运行频率40MHz。
步骤(2)无线定位模块的车载定位模块中的控制器从非易失性存储器中读取场地环境对应的标定参数,标定参数包括路径衰减指数λ、标准偏差为σ的正态随机变量ξσ、d0米处引导模块收到的RSSI信号强度值RSSI(d0);λ、ξσ、RSSI(d0)与外界环境密切相关,可采用最小二乘法根据场地环境拟合得到;标定参数的过程:固定低频发射基站,在距低频发射基站0至10m之间,每隔20cm的距离选取一个测试点,引导模块位于测试点处,测试低频发射基站在发射RSSI信号强度值恒定为RSSI(C)时,引导模块在测试点处收到的RSSI信号强度值,测得一系列强度值,采用最小二乘法对λ、ξσ进行拟合,选取d0为5m,测试引导模块在该点收到的RSSI信号强度值RSSI(d0),把λ、ξσ、RSSI(d0)存储于MC9S12XEP100的非易失性存储器中。
步骤(3)车载定位模块等待来自电机控制模块的定位启动命令;如果收到电机控制模块的定位启动命令,则执行步骤(4),如果未收到电机控制模块的定位启动命令,则重复本步骤;
步骤(4)车载定位模块通过ISM(433.05MHz~434.79MHz)频段的高频无线电信号向引导模块发送初始化命令,使能引导模块初始化;引导模块初始化内部的控制器2-3中的单片机时钟频率、输入输出、内部变量、寄存器和中断,使能引导模块上的三维低频接收天线;ISM频段的高频无线电信号频率为采用FSK调制方式。引导模块内部的控制器选用Freescale公司的MC9S08AC16,CPU运行频率32MHz,CPU总线频率16MHz,该控制器包含丰富的输入输出、定期器和中断资源。引导模块上的三维低频接收天线,由于采用三维结构,可以很好的接收三维空间的低频无线信号,降低了低频无线接收时对引导模块放置姿态的要求。
步骤(5)车载定位模块包含N(N≥3)个低频发射基站,其三维坐标依次为(x1,y1,z1)、(x2,y2,z2)、(x3,y3,z3)…(xN,yN,zN);关闭所有N个低频发射基站的低频发射功能,使能第n(n≤N)个低频发射基站的低频发射功能,发射频率恒定为fc,发射RSSI信号强度值恒定为RSSI(C);
车载定位模块通过ISM频段的高频无线电信号向引导模块发送接收命令;引导模块开始对频率为f
c的低频无线电信号的接收,得到接收RSSI信号强度值RSSI(d
0);引导模块通过ISM频段的高频无线电信号向车载定位模块发回RSSI(d
n),车载定位模块记录该值并保存于内存中;对RSSI(d
n),车载定位模块根据公式
计算引导模块距第n(n≤N)个低频发射基站的距离d
n;一般情况下N选取6,既可以提高后续计算的精度,也可以降低运算量,f
c为125KHz。
步骤(6)延时时间30毫秒,重复执行步骤(5)直到得到N个低频发射基站中每个低频发射基站对应的距离d1、d2、d3...dN-1、dN;
以任意一个低频发射基站为原点,建立空间三维坐标系,则d1、d2、d3...dN-1、dN分别对应N个低频发射基站三维坐标(x1,y1,z1)、(x2,y2,z2)、(x3,y3,z3)…(xN,yN,zN);
步骤(7)从步骤(6)得到的N个距离值里挑选出3个距离值,一共有
种挑选方案,依次编号为
假设其中某一方案m
中的3个距离值为d
m1、d
m2、d
m3,对应的三个低频发射基站坐标记为(x
m1,y
m1,z
m1)、(x
m2,y
m2,z
m2)、(x
m3,y
m3,z
m3);
解方程组
得到一个定位出的引导模块的三维坐标值(x
m,y
m,z
m)。
步骤(8)重复执行步骤(7)
次,得到
个引导模块的三维坐标值
步骤(9)对步骤(8)得到的
个引导模块的三维坐标值,根据如下公式计算
结果(x
r,y
r,z
r)为最终的引导模块的三维坐标值;
步骤(10)车载定位模块向电机控制模块发送定位结果(xr,yr,zr);电机控制模块收到定位结果(xr,yr,zr)后,可以计算出引导模块的方位角及距离,并根据该方位角和距离作出决策,驱动电机转向或前进。
步骤(11)重复步骤(5)至步骤(10),直至车载定位模块的电源被关闭;车载定位模块大约每隔0.6s向外发送一次定位结果,选取较少的低频定位基站可以减小向外发送定位结果的时间间隔,并提高系统反应速度,使得电机控制模块的控制动作更加精细。