The Autonomous Underwater Vehicle (AUV) is usually equipped with multiple sensors, such as an inertial navigation system (INS), ultra-short baseline system (USBL), and Doppler velocity log (DVL), to achieve autonomous navigation. Multi-source information fusion is the key to realizing high-precision underwater navigation and positioning. To solve the problem, a fusion scheme based on factor graph optimization (FGO) is proposed. Due to multiple iterations and joint optimization of historical data, FGO could usually show a better performance than the traditional Kalman filter. In addition, considering that USBL and DVL are usually heavily influenced by the environment, outliers are often present. A robust integrated navigation algorithm based on a maximum correntropy criterion and FGO scheme is proposed. The proposed algorithm solves the problem of multi-sensor fusion and non-Gaussian noise. Numerical simulations and field tests demonstrate that the proposed FGO scheme shows a better performance and robustness than the traditional Kalman filter. Compared with the traditional Kalman filtering, the positioning accuracy is improved by 5.3%, 9.1%, and 5.1% in the east, north, and height directions. It can realize a more accurate navigation and positioning of underwater multi-sensors.

Angle-of-arrival (AOA) measurements are often used in underwater acoustical localization. Different from the traditional AOA model based on azimuth and elevation measurements, the AOA model studied in this paper uses bearing measurements. It is also often used in the Ultra-Short Baseline system (USBL). However, traditional acoustical localization needs additional range information. If the range information is unavailable, the closed-form solution is difficult to obtain only with bearing measurements. Thus, a localization closed-form solution using only bearing measurements is explored in this article. A pseudo-linear measurement model between the source position and the bearing measurements is derived, and considering the nonlinear relationship of the parameters, a weighted least-squares optimization equation based on multiple constraints is established. Different from the traditional two-step least-squares method, the semidefinite programming (SDP) method is designed to obtain the initial solution, and then a bias compensation method is proposed to further minimize localization errors based on the SDP result. Numerical simulations show that the performance of the proposed method can achieve Cramer–Rao lower bound (CRLB) accuracy. The field test also proves that the proposed method can locate the source position without range measurements and obtain the highest positioning accuracy.

In the functioning of the hand–eye collaboration of an apple picking robot, the accuracy of the hand–eye relationship is a key factor affecting the efficiency and accuracy of the robot’s operation. In order to enhance the low accuracy of traditional hand–eye calibration methods, linear and nonlinear solving methods based on mathematical tools such as quaternions are commonly adopted. To solve the loss of accuracy in decoupling during the linearization solution and to reduce the cumulative error that occurs during nonlinear solutions, a hand–eye calibration method, based on the ICP algorithm, is proposed in this paper. The method initializes the ICP matching algorithm with a solution derived from Tsai–Lenz, and substitutes it for iterative computation, thereby ascertaining a precise hand–eye conversion relationship by optimizing the error threshold and iteration count in the ICP matching process. Experimental results demonstrate that the ICP-based hand–eye calibration optimization algorithm not only circumvents the issues pertaining to accuracy loss and significant errors during solving, but also enhances the rotation accuracy by 13.6% and the translation accuracy by 2.47% compared with the work presented by Tsai–Lenz.

An accurate map is needed for the autonomous navigation of mobile robots in unknown environments. The application of laser radars has the advantages of high ranging accuracy and long ranging distances. Due to the small amount of data on laser radars and the influence of noise on the sensor itself, these amount to causing problems such as low accuracies of map construction and large positioning errors. Currently, the feature extraction of environmental line segments based on radar scanning data generally adopts the idea of recursion. However, the amount of calculations for applying recursion is large, and the threshold of extracted feature points needs to be set manually. Moreover, the fixed segmentation threshold will cause under-segmentation or over-segmentation. In this paper, an adaptive threshold-based feature extraction method for environmental line segments is proposed. The method denoises the original data first, and then an adaptive threshold of the nearest neighbor algorithm is provided to improve the accuracy of breakpoint judgment; next, the slope difference between adjacent line segments is evaluated according to the line segment fitting error in order to obtain the optimal corner feature. Finally, the point set is segmented to fit line-segment features. Based on actual environment tests, the environmental similarity of the line segment features extracted by the new algorithm in this paper increases by 8.3% compared with the IEPF (Iterative End Point Fit) algorithm. The algorithm avoids recursive operations, improves the efficiency by four times, and meets the real-time requirements of line segment fitting.