主从式无人水面艇协同定位滤波方法与实验验证

多无人水面艇（USV）协同导航定位技术是解决复杂作业环境下导航问题的重要途径。针对单主艇的主从式多USV的协同导航定位问题，建立了协同定位的状态空间模型，然后将非线性模型绕滤波值展开，得到非线性系统的线性化模型；利用扩展卡尔曼滤波算法进行信息融合；针对双艇间的距离、角度与协同定位精度的关系展开研究，对协同导航滤波算法使用李代数法进行了可观测性分析。研究表明，控制相邻两时刻主从艇之间距离和角度变化量是提高协同定位精度的有效措施。通过仿真和实验验证，为下一步深入研究和路径规划奠定了基础。

A Cooperative Navigation Approach and Its Verification of USVs with Leader-fellower Structure

The cooperative navigation and localization technology of multi-USVs is an important way to solve the navigation problem in complex environment. For the cooperative navigation and localization of multi-USVs with leader-follower structure, a state space model of cooperative localization is built, and then a linear model is obtained by expanding the nonlinear model around filter value. An extended Kalman filter is designed to fuse the sensor information to locate the slave USV in real time. At last, the relationship among USV distances, angle and accuracy is researched, and the observability analysis of cooperative navigation filter algorithm is done with Lie algebra. The results show that the navigation accuracy is improved effectively by controlling the variations of distances and angles of leader and slave USVs between two adjacent moments, which is verified by simulation and experiment.