Dynamic Parallel Mapping and Trajectory Planning of Robot Arm in Unknown Environment

A high real-time robot arm mapping and dynamic shortest path planning algorithm is proposed, which is used for low-cost depth cameras and robot arms. First of all, an inverse kinematics solution algorithm for a nonspherical wrist joint is proposed for the Kinova Mico2 manipulator. Based on the idea of position and attitude decoupling combined with the numerical solution, the solution of multiple sets of inverse kinematics is acquired. Second, a hand–eye system is established on the basis of the depth camera—RealSense. The point cloud is inversely solved through the depth map, which is preprocessed by the pass-through filtering and outlier removal algorithm. A scene parallel mapping method is designed to achieve the real-time reconstruction of the working scene for the robot arm, in which the maintenance of the map and large matrix operations can be completed by GPU. Third, a path planning algorithm with the shortest path for the manipulator end in Cartesian space is designed based on the $\text{D}\ast $ method and grid map. The attitude interpolation and smoothing are performed on the path points to obtain the executable joint trajectories on the time axis. Finally, the robot operating system (ROS) is used to build a simulation test verification environment, and the effectiveness of the related algorithms is verified through multiple simulation cases.