基于全质心WLS-HDS-TWR算法的无人驾驶农业机械协同定位算法研究

针对室内全球导航卫星系统(Global navigation satellite system,GNSS)信号受遮挡时,农用车辆协同定位精度低、稳定性差、信号丢包等问题,本文开展面向超宽带(Ultra-wideband,UWB)调频技术的室内外农用车辆协同定位算法研究。首先,搭建三基站多边测距定位模型,实现主基站绝对位置标定以及辅助基站绝对位置坐标的变换求解;其次,提出全质心加权最小二乘的高速双边双向(Weighted least squares high double sided two-way ranging,WLS-HDS-TWR)农机协同定位算法,基于泰勒级数展开的WLS估计算法,求解主车位置。同时,提出面向室内环境的多状态基站组合的UWB定位模块布设模式,并验证其可行性;通过飞行时间法(Time of flight,TOF)获取主从车距离信息,融合GNSS标定位置信息、主车坐标信息以及测距信息,实现主从车协同定位。最后,基于Prescan/Simulink搭建联合仿真平台,验证提出算法的可靠性;通过农用履带车辆开展室内及室外协同定位实车试验,试验结果表明:全质心WLS-HDS-TWR协同定位算法可有效解决室内GNSS信号缺失问题,室内环境下,定位精度较HDS-TWR及全质心LS-HDS-TWR算法分别提高26.98%和22.03%,满足智能农机协同定位作业需求。