基于全质心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%，满足智能农机协同定位作业需求。

Cooperative Localization Algorithm for Full Center Mass of WLS-HDS-TWR Driverless Agricultural Machines

Aiming at the problems of low accuracy, poor stability, and packet loss of agricultural machines indoor localization when the signal transmission of global navigation satellite system (GNSS) was obstructed. The research of indoor and outdoor cooperative agricultural machines localization algorithm for ultra-wideband (UWB) frequency modulation technology was carries out. Firstly, the three base-station multilateral ranging and localization model was constructed, the absolute localization solve of the main base station and the transformation of the coordinates of the auxiliary base station were realized. Meanwhile, the weighted least squares high double sided two-way ranging (WLS-HDS-TWR) with the full center mass was proposed, and the WLS estimation algorithm based on Taylor series expansion solved for the center of mass of the triangle formed by the intersection of the base station ranging circles, i.e., the localization of the active vehicle. The indoor deployment mode of UWB positioning module for multi-state base station combination was innovatively proposed. The distance information of master and slave vehicle was obtained by time of flight (TOF) method. By integrating GNSS calibration position information, master vehicle coordinates information, and distance measurement information, master-slave vehicle cooperative localization was achieved. Finally, a joint simulation platform was built based on Prescan/Simulink to verify the reliability of the proposed algorithm. The indoor and outdoor collaborative localization experiments were carried out by agricultural tracked vehicles. The experiments showed that the full center mass of WLS-HDS-TWR collaborative positioning algorithm can effectively solve the problem of indoor GNSS signal loss, and the positioning accuracy was improved by 26.98% and 22.03% compared with the HDS-TWR and the full center mass of LS-HDS-TWR algorithms, meeting the needs of intelligent agricultural machines collaborative positioning.