Patent ID: 11892855
Assignee: ZHEJIANG UNIVERSITY
Field: Measurement (Instruments)
Classification: CPC G  H | IPC G  H

Claim 5:
6. An autonomous mapping method for the robot with perception capability of livestock and poultry information based on autonomous navigation according to claim 1, the method comprising the steps of:
(S1) controlling the robot to move in indoor working environment, and simultaneously obtaining information about surrounding environment during movement with the LiDAR, the RGB-D camera, the inertial measurement unit and the odometer, wherein the information comprises obstacle distance information, image and depth information, pose information and odometer information in a local coordinate system, the pose information comprises first real-time global coordinates, the odometer information comprises second real-time global coordinates, speed, heading angle and wheel angular velocity;
(S2) receiving and processing the information about the surrounding environment through the main control module, obtaining positioning global coordinates, speed, heading angle and wheel angular velocity of the robot in a world coordinate system by coordinate transformation;
(S3) defining the positioning global coordinates, speed, heading angle and wheel angular velocity of the robot in the world coordinate system to be a state vector of a Kalman filter, wherein the positioning global coordinates in the world coordinate system are obtained through processing the first real-time global coordinates and the second real-time global coordinates in the local coordinate system;
(S4) constructing a state model of the Kalman filter according to the state vector, constructing an observation model of the Kalman filter according to an observation model of the odometer, an observation model of the inertial measurement unit and an observation model of the LiDAR, solving the state model and the observation model of the Kalman filter according to Kalman filter algorithm, and obtaining an optimal solution of the state vector at time t;
(S5) determining the optimal solution of the state vector under the state model and the observation model of the Kalman filter in the step (S4) in combination with image information collected by the RGB-D camera and Monte Carlo real-time positioning and mapping algorithm, which comprise:
(S501) the robot moving in an area to be constructed on a map, judging whether the robot turns and whether there are obstacles during movement through the obstacle distance information collected by the LiDAR, judging whether there are characteristic road markings are captured through the image information collected by the RGB-D camera, performing feature matching on information collected by the LiDAR, the inertial measurement unit and the odometer in the area to be constructed on the map, and obtaining poses in the world coordinate system, wherein the poses are the global coordinates of the robot in the world coordinate system; and
(S502) when the robot does not turn, no obstacles or no characteristic road markings are captured by the RGB-D camera during the movement of the robot, defining control vectors of the state model of the Kalman filter to be the poses in the world coordinate system;
when the robot turns, there are obstacles or characteristic road markings are captured by the RGB-D camera during the movement of the robot, defining the control vectors of the state model of the Kalman filter to be the optimal solution of the state vector; and
(S6) iteratively solving the state model and the observation model of the Kalman filter, and obtaining locations, so that a global map is constructed.