Research on Intelligent Position Posture Detection and Control based on Multi-sensor Fusion Method

Aiming at the problems of low efficiency and high safety risks encountered in grabbing and placing the boxes by manual mode, the multi-sensor fusion method of vision and laser ranging sensor is used for position and posture detection, and the global rough positioning is detected by the global binocular camera. Precise positioning uses four local measurement modules, including a monocular camera and three laser ranging modules, to detect the position and posture of the target, and uses Move it in ROS for trajectory planning, obstacle avoidance detection and object grabbing. The intelligent position posture detection and control technology proposed in this paper can effectively improve the timeliness of the box grabbing operation, ensure the safety of the box grabbing operation, and enhance the adaptability to harsh environments.


Introduction
In 2012, the Curiosity rover of American landed on the surface of Mars and completed the tasks of walking, surveying, and drilling in harsh environments. Intelligent robotics technology began to develop rapidly. In 2016, Tesla Motors in the United States put into full-line production of unmanned vehicles. In 2020, Baidu also announced the realization of mass production of unmanned vehicles. Some well-known universities in the world, such as Carnegie Mellon University and Massachusetts Institute of Technology, are studying intelligent engineering. Mechanical technology, the key technologies such as stereo vision, deep learning, radar scanning, and intelligent control used in these research fields will be introduced into the automatic intelligent filling of the box as soon as possible, so as to provide advance technical reserves for the automation, intelligence, and unmanned filling of the filling process. The method improves the timeliness of the box filling operation, ensure the safety of the box filling operation, and enhance the adaptability of the box filling operation to harsh environments. A rotor UAV system was designed and implemented to mitigate the real-time autonomous navigation problem of UAVs [1]. The indoor three dimensional (3D) measurement method based on binocular technology, and acquires the 3D information of the indoor target was studied [2]. In [3], a binocular vision tracking and positioning method for mine moving target based on ORB feature was proposed. The manipulator control system based on the ROS. Using the binocular stereo vision avoids obstacles and grasps object based on the traditional manipulator control system was designed [4].

Intelligent loading position detection and control system design
The intelligent loading posture detection system is mainly used to detect the three-dimensional position and posture information of the target, and feed it back to the computer control system to realize the automatic control of the robotic arm grasping and reloading. The pose detection system mainly includes a set of binocular vision detection device, a set of monocular vision detection device and three sets of laser distance measuring devices. In the initial position, the binocular vision detection device recognizes the shape characteristics of the box bomb module and the target loading frame, and performs the pose calculation, and guides the robotic arm to reload the box bomb module to the vicinity of the target loading frame (make sure to enter the monocular The effective field of view of the camera), that is, the rough positioning of the loading is completed; after that, the three sets of laser ranging devices detect the vertical distance between the manipulator and the loading target frame, and calculate the pose through the feedback to the control system to complete the box bomb The parallel alignment of the module and the target loading frame eliminates the inclination error; finally, the monocular vision device calculates the plane alignment error of the box bomb module by matching the visual mark of the loading target position, and realizes the box bomb module and the loading frame position. The vertical, horizontal, and deflection alignment of the box, so as to finally complete all the filling and alignment actions, and realize the automatic filling of the box bomb module in the whole process. The composition of the intelligent position posture detection and control platform is shown in Figure 1.

Rough target positioning based on binocular camera
The module first uses a vision-based method to identify the goods or the shelf to be loaded, and obtain the three-dimensional bounding box of the object in the space. After that, the object pose estimation method is used to estimate the 6D pose of the goods or shelves, and the ICP-based method is used to further optimize the result of the pose estimation, so that the horizontal accuracy can reach ±25mm. The process is divided into four parts shown in Figure 2. (2) Based on the target detection and semantic segmentation results given in the first step, use the deep learning method PVN3D based on key point detection to return the 6D pose of the object coordinate system in the camera coordinate system as the result of object pose estimation; (3) Use the ICP method (cupoch) to further optimize the result of the object pose estimation to improve the accuracy of the pose estimation to ±25mm; (4) Perform continuous coordinate system transformation on the optimized posture estimation results to obtain the posture of the goods to be picked up/the rack to be loaded in the robotic arm coordinate system, and according to the relative position of the end effector and the target position under the grab action Relationship, determine the spatial coordinates and attitude of the end position servo of the robotic arm.
After epipolar rectification, the left and right images are subjected to surf binocular stereo matching, and the stereo matching effect diagram is shown in Figure 3.

Three-point leveling based on laser rangefinder
The three-point leveling process calculates the relative position relationship between the target and the installation point of the rangefinder in the robot arm coordinate system, and uses the analytical geometry method to calculate the position servo target of the robot arm during three-point leveling. The three-point leveling process based on the laser rangefinder is shown in Figure 5. (2) Read the distance readings of each laser rangefinder, combine the spatial coordinates and pointing of each end effector in the robotic arm coordinate system, establish the spatial position model of each laser projection point in the robotic arm coordinate system, and then obtain the target The relative positional relationship between the target plane and the fixture plane.
(3) Calculate the transformation matrix between the two triangles according to the relative positional relationship between each target point on the target plane and each rangefinder installation point on the fixture plane, so that the two planes are parallel and the line of each corresponding point is perpendicular to the two planes; (4) Transform the space transformation matrix of the installation plane of the rangefinder calculated in 3) to obtain the motion target of the end effector of the robotic arm, and execute the corresponding action.
In order to avoid the risk that the laser rangefinder cannot hit the target and cause the rotation relationship to be too large, this module needs to rely on the camera model based on the monocular camera to estimate the distance from the focal plane of the camera to the target. When the spatial distance exceeds the threshold, the three-point leveling is temporarily closed, and a more precise positioning process is performed.

Precise target positioning based on monocular camera
The precise positioning process of the target is realized by the method of visual servoing, which is shown in Figure 6. Through the process of iterative optimization, the centre of the camera screen is gradually coincided with the centre of the target QR code, and the orientation of the camera plane is consistent with the preset direction of the target QR code. (2) Establish a camera model based on the camera's internal parameter matrix, estimate the distance from the camera to the target by the size of the target in the field of view, and convert the 5mm target error to the distance threshold on the camera plane; (3) Use OpenCV to obtain the position and orientation of the target QR code in the field of view of the monocular camera on the focal plane of the camera, generate the movement direction and step length of the camera in the next process, and perform the horizontal direction of the end fixture of the robotic arm (by The three-point leveling process of laser ranging determines the visual servoing in the horizontal direction until the distance between the center point of the camera screen and the center point of the target is less than the threshold; (4) Iteratively carry out the process of horizontal visual servo-horizontal leveling until the requirements of horizontal displacement error and distance error of each laser ranging point are met at the same time.

Mechanical Arm control based on Position Posture
The motion control scheme uses Moveit in ROS for trajectory planning, obstacle avoidance detection and object grabbing. Motion control framework by mechanical arm is shown in Figure 7.
(1) Model establishment. The model is divided into the establishment of the robot body model and the establishment of the workspace model. Among them, the model of the robotic arm will be provided by the URDF agent, and the model of the working space needs to be built by yourself.
(2) Obstacle avoidance setting. There is a module structure for planning scene listeners in Moveit, which can be used to detect whether there are obstacles in the robot scene. Obstacles can be told to the listener in several ways: add them through the Rviz interface; add them through programming (C++/Python); the information detected in real time by the robot's external sensors, such as the Kinect depth camera, is added to the scene.
(3) Trajectory planning. Use open source OMPL for mechanical trajectory planning. By setting the target pose and motion constraints, the motion plan of the robotic arm can be calculated and the motion control of the robotic arm can be realized.
(4) Object grabbing. Object grabbing needs to cooperate with vision and sensor data. After reaching the target position, control the movement of the manipulator to complete the grabbing operation.

Conclusion
The intelligent loading experiment platform uses a manipulator to grab and load the box bomb module. Based on non-contact measurement technologies such as vision and laser, it obtains three-dimensional data of the target in real time, and transmits it to the intelligent control platform for real-time pose calculation to realize the robotic arm. The feedback control of the end manipulator completes the task of automatic loading and reprinting.
(1) Two-level control strategy combining global coarse positioning and local fine positioning The robot arm control of the intelligent loading platform adopts a control strategy that combines global coarse positioning and local fine positioning: global coarse positioning uses a global binocular camera for position and posture detection, and guides the robot arm into the field of view of the fine positioning detection camera; fine positioning passes four A local measurement module (including a monocular camera and three laser ranging modules) detects the position and posture of the target, and completes the final automatic filling and alignment.
(2) Detection methods of multi-sensor fusion such as binocular vision + monocular vision + laser ranging Using vision + laser ranging sensor multi-sensor fusion method for position and posture detection: The laser ranging sensor detects the space posture of the manipulator, realizes the manipulator roll, pitch and vertical adjustment control.The visual sensor detects the horizontal position of the manipulator, which realizes the horizontal, longitudinal and yaw angle adjustment control of the manipulator.