Multi-source fusion robot indoor absolute positioning method based on filtering

By fusing pseudosatellite sensors with visual and inertial sensors and employing the extended Kalman filter method, the problem of accumulated visual and inertial navigation errors in indoor robot positioning and navigation was solved, achieving high-precision absolute positioning of the robot both indoors and outdoors.

CN114812554BActive Publication Date: 2024-11-19THE 54TH RESEARCH INSTITUTE OF CHINA ELECTRONICS TECHNOLOGY GROUP CORPORATION
View PDF 0 Cites 0 Cited by

Patent Information

Application Number
CN202210297722.2
Authority / Receiving Office
CN · China
Patent Type
Patents(China)
Current Assignee / Owner
Filing Date
2022-03-25
Publication Date
2024-11-19
Estimated Expiration
2042-03-25

AI Technical Summary

Technical Problem

In indoor positioning and navigation, robots suffer from unreliable positioning results and difficulty in calculating absolute coordinates due to the accumulation of errors from visual odometry and inertial navigation, as well as GNSS signal obstruction.

Method used

By introducing pseudo-satellite sensors and combining them with vision and inertial sensors for multi-source fusion, and using the extended Kalman filter method to correct vision and inertial navigation based on pseudo-satellite observation data, continuous indoor and outdoor positioning of the robot can be achieved.

Benefits of technology

It improves the stability and accuracy of robot positioning and navigation, reduces drift in visual and inertial navigation, and achieves high-precision absolute positioning both indoors and outdoors.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN114812554B_ABST
    Figure CN114812554B_ABST
Patent Text Reader

Abstract

The present invention discloses a method for indoor absolute positioning of a robot based on filtering and multi-source fusion, and belongs to the field of robot positioning and navigation technology. The traditional vision and IMU fusion solution usually only estimates the robot's posture relative to the local coordinate system, and the posture estimation result drifts over time. The present invention adds a pseudo-satellite sensor, which can obtain the absolute coordinate position of the robot relative to the satellite navigation coordinate system, and then performs loose coupling fusion with the vision sensor and IMU sensor module under the extended Kalman filter framework to correct the accumulated error of vision and IMU over time, and obtain a robust robot absolute posture estimation result. The present invention realizes the unification of the robot's indoor and outdoor coordinate references, as well as the seamless positioning of the robot indoors and outdoors, thereby improving the integrity and stability of the robot's positioning and navigation system.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The present invention relates to the field of robot positioning and navigation, in particular to the field of robot indoor positioning and navigation, and more particularly to the field of robot multi-source fusion indoor positioning and navigation. Background Art

[0002] At present, vision and IMU fusion based on loosely coupled filtering is a common solution for robot positioning and navigation. This solution fuses the pose-related information calculated by the visual sensor and the IMU sensor to obtain a more robust and accurate pose. However, due to factors such as the accumulation of errors between the visual odometer and inertial navigation over time, the pose results obtained by fusion are not reliable. In addition, due to the obstruction of indoor GNSS signals, the robot cannot obtain the absolute coordinates in the satellite navigation coordinate system indoors and can only calculate the coordinates of the robot's local coordinate system.

[0003] To address these issues, a solution is needed to enable GNSS signals to help robots locate and navigate indoors. Pseudo-satellites, as a supplement to outdoor navigation satellites, can emit navigation signals similar to GNSS, which can not only support robots' navigation and positioning outdoors, but also enable navigation and positioning in environments where GNSS signals are blocked and interfered with, such as mountainous canyons, underground, tunnels, and indoors. Summary of the invention

[0004] The present invention introduces a pseudo-satellite sensor module, and fuses the posture data observed by the pseudo-satellite with the posture-related information of the visual odometer and the IMU sensor to solve the problem of continuous positioning and navigation of the robot indoors and outdoors. The posture data observed by the pseudo-satellite can correct the visual odometer and inertial navigation to a certain extent, further improving the stability and accuracy of the robot's positioning and navigation system.

[0005] The technical solution adopted by the present invention is:

[0006] A multi-source fusion robot indoor absolute positioning method based on filtering includes the following steps:

[0007] ① Select a known point in the experimental site as the initial position of the robot's movement, and deploy a pseudo-satellite base station in the experimental site to transmit satellite navigation messages;

[0008] ② Install pseudo-satellite sensors, visual sensors and inertial sensors on the robot platform and calibrate them respectively; after installation, start the pseudo-satellite sensors, visual sensors and inertial sensors to control the robot to move from the initial position;

[0009] ③ During the robot's movement, the processor receives the observation data sent back by the pseudo-satellite sensor, visual sensor and inertial sensor, and analyzes the observation data;

[0010] ④ Use the parsed data to perform extended Kalman filtering and finally estimate the robot’s fusion posture.

[0011] Furthermore, the specific process of step ① is: selecting an indoor area as an experimental site, measuring a known point in the site as the initial position of the robot; deploying no less than 3 pseudo-satellite base stations in the experimental site, and the pseudo-satellite base stations are used to transmit satellite navigation messages to pseudo-satellite sensors.

[0012] Furthermore, in step ②, the specific process of installing the pseudo-satellite sensor, the visual sensor and the inertial sensor on the robot platform and calibrating them respectively is as follows: the pseudo-satellite sensor is fixed on the top of the robot body to obtain the satellite navigation message transmitted by the pseudo-satellite base station; the visual sensor is installed and fixed on the top edge of the robot body, facing the front of the robot's travel direction, and ensuring that there is no obstruction in front of the lens, so as to capture the image frame; the inertial sensor is fixed horizontally on the robot body to collect the linear velocity and angular velocity information of the robot, and the relative postures of the three sensors are calibrated respectively.

[0013] Furthermore, the specific process of step ③ is as follows: during the movement of the robot, the pseudo-satellite sensor receives the satellite navigation message transmitted by the pseudo-satellite base station, performs signal processing to obtain the longitude, latitude and altitude data in the geodetic coordinate system, the visual sensor obtains the image sequence, and the inertial sensor obtains the linear velocity and angular velocity of the image interval; the processor converts the longitude, latitude and altitude data obtained by the pseudo-satellite sensor into the UTM coordinate format, converts them into coordinate data in the world coordinate system, solves the image sequence obtained by the visual sensor to obtain the posture change between two consecutive frames of images, solves the linear velocity and angular velocity of the image interval obtained by the inertial sensor to obtain a quaternion, and converts them into the attitude angle of the robot.

[0014] Furthermore, the extended Kalman filter in step ④ specifically includes the following steps:

[0015] a) Establish the robot motion state equation:

[0016] x k =f(x k-1 )+w k-1

[0017] Among them, x k is the motion state of the robot at time k, x k-1 is the motion state of the robot at time k-1, f is the nonlinear conversion function from the motion state of the robot at time k-1 to the motion state at time k, and w k-1 is process noise, which conforms to normal distribution;

[0018] b) Establish the measurement equation of the robot:

[0019] Z k =h(x k )+v k

[0020] in, represents the measurement value of the inertial sensor at time k, h imu (x k ) represents the measurement model that maps the robot motion state to the inertial sensor measurement space, represents the normally distributed measurement noise of the inertial sensor, represents the measurement value of the visual sensor at time k, h vis (x k ) represents the measurement model that maps the robot motion state to the visual sensor measurement space, represents the normally distributed measurement noise of the vision sensor, represents the measurement value of the pseudo-satellite sensor at time k, h psl (x k ) represents the measurement model that maps the robot motion state to the pseudo-satellite sensor measurement space, is the normally distributed measurement noise of the pseudolite sensor;

[0021] c) Execute robot state and covariance prediction:

[0022]

[0023]

[0024] in, is the robot motion state predicted at time k, is the estimated error covariance of the k-time forecast, P k-1 is the estimated error covariance at time k-1, f(x k-1 ) is the nonlinear conversion function from the robot motion state at time k-1 to the motion state at time k, the estimated error covariance is projected through F, Q is the process noise covariance, and F is the Jacobian matrix of f;

[0025] d) Execute the correction of the robot's motion state:

[0026]

[0027]

[0028]

[0029] Where K represents the Kalman gain, H is the observation matrix, and R is the measurement covariance matrix. is the estimated error covariance predicted in step c), Z is the observed value of the robot's inertial sensor, visual sensor and pseudo-satellite sensor at time k, is the robot motion state predicted in step c), P k is the covariance matrix of the robot motion state after correction, x k It is the robot motion state after final correction.

[0030] The advantages of the method of the present invention are as follows:

[0031] 1. The present invention proposes a multi-source fusion robot indoor absolute positioning method based on filtering. This method innovatively integrates visual sensors, inertial sensors and pseudo-satellite sensors in an extended Kalman filter framework, effectively solving the problem of the robot's difficulty in performing absolute positioning indoors and improving the continuity of indoor and outdoor positioning and navigation.

[0032] 2. The pose data obtained by the pseudo-satellite sensor in the present invention can correct the pose information calculated by the visual sensor and the inertial sensor, thereby reducing the drift of the visual and IMU pose estimation caused by accumulation over time.

[0033] 3. The method proposed in the present invention is simple to implement, has low computational complexity, low cost, and can be easily integrated into small mobile devices, providing a new idea for solving the problem of high-precision absolute positioning of robots indoors. BRIEF DESCRIPTION OF THE DRAWINGS

[0034] Figure 1 It is a flow chart for implementing the method of the present invention.

[0035] Figure 2 It is a schematic diagram of the fusion principle of the extended Kalman filter of the present invention. DETAILED DESCRIPTION

[0036] The technical solution of the present invention is further described below in conjunction with the accompanying drawings.

[0037] like Figure 1 As shown, the indoor absolute positioning method of multi-source fusion robot based on filtering includes the following steps:

[0038] 1. Build an indoor robot experimental environment and select an area indoors as the experimental site. Use a total station to measure a known point in the site as the robot's initial position, and set the robot's initial position. Deploy no less than three pseudo-satellite base stations in the site. The pseudo-satellite base stations include multi-channel signal transmitters and receivers. The pseudo-satellite base stations are used to transmit satellite navigation messages to pseudo-satellite sensors.

[0039] 2. Install pseudo-satellite sensors (pseudo-satellite user equipment), visual sensors (monocular cameras) and inertial sensors (IMU sensors) on the robot platform. The pseudo-satellite user equipment is fixed on the top of the robot body, including a pseudo-satellite receiving antenna and a pseudo-satellite navigation chip. The pseudo-satellite receiving antenna is used to receive the pseudo-satellite navigation signal transmitted by the pseudo-satellite base station and send it to the pseudo-satellite navigation chip. The pseudo-satellite navigation chip is used to parse and process the signal, obtain pseudo-satellite observation data such as carrier phase measurement values ​​and pseudo-range measurement values, construct pseudo-range observation equations and perform parameter calculations, so as to solve the geodetic coordinates of the pseudo-satellite user equipment. The monocular camera is fixed on the top of the robot, facing the front of the robot's travel direction, and ensuring that there is no obstruction in front of the lens, for capturing image frames. The IMU sensor is also fixed horizontally on the robot body to collect information such as the robot's linear velocity and angular velocity. Then, the external parameters of the monocular camera, IMU sensor and pseudo-satellite user equipment are calibrated to estimate the relative position and posture of the three. Next, the pseudo-satellite user equipment, monocular camera and IMU device are started, and the robot is remotely controlled to move from the initial point with a joystick or a notebook.

[0040] 3. During the movement, the processor receives the observation data sent back by the pseudo-satellite user device, monocular camera and IMU, and uses the ROS platform to synchronize the timestamps of the information collected by the pseudo-satellite user device, monocular camera and IMU. The observation data obtained by the pseudo-satellite user device is the longitude, latitude and altitude data in the geodetic coordinate system, and the ROS platform is used to convert the UTM coordinate format into coordinate data in the world coordinate system. The monocular camera sends back an image sequence, and the processor corrects and detects features on the image, estimates the basic matrix, and solves the pose change between two consecutive frames of images through singular value decomposition; selects the linear velocity and angular velocity of the image interval sent back by the IMU, and solves the quaternion from the initial attitude angle, which is further converted into the robot's attitude angle.

[0041] 4. The processor performs extended Kalman filter fusion on the parsing results of the three observation data, and finally estimates the fusion posture of the robot, such as Figure 2 As shown, the specific steps are as follows:

[0042] 1) Establish the motion state equation of the robot between adjacent moments (time t-1 and time t):

[0043] x k =f(x k-1 )+w k-1 (1)

[0044] Among them, x k is the motion state of the robot at time k, x k-1is the motion state of the robot at time k-1, f is the nonlinear conversion function from the motion state at time k-1 to time k, and w k-1 represents process noise, which conforms to the normal distribution. In this embodiment, the inertial sensor, visual sensor and pseudo-satellite sensor are integrated to obtain a maximum of 15-dimensional motion state vector of the robot, so x k and x k-1 Written in coordinate component form:

[0045]

[0046]

[0047] In formula (2) and formula (3), (x k ,y k ,z k ) and (x k-1 ,y k-1 ,z k-1 ) represent the positions of the robot in the world coordinate system at time k and time k-1 respectively, (α k ,β k ,γ k ) and (α k-1 ,β k-1 ,γ k-1 ) represent the posture of the robot in the world coordinate system at time k and time k-1 respectively, (vx k ,vy k ,vz k ) and (vx k-1 ,vy k-1 ,vz k-1 ) is the velocity of the robot carrier coordinate system at time k and time k-1, and represents the time derivative of the posture in the robot carrier coordinate system at time k and time k-1, (ax k ,ay k ,az k ) and (ax k-1 ,ay k-1 ,az k-1 ) is the acceleration of the robot in the carrier coordinate system at time k and time k-1. In practical problems, all 15-dimensional vectors of the robot are often not needed, and can be selected as needed.

[0048] In this embodiment, equation (1) can be specifically written as a position motion equation and a posture motion equation. The specific form of the position motion equation is as follows:

[0049]

[0050] The matrix A represents the transformation matrix between the world coordinate system and the carrier coordinate system.

[0051] Among them, the specific form of the robot's posture motion equation is as follows:

[0052]

[0053] Among them B 3×3 Represents the transformation matrix between the world coordinate system and the carrier coordinate system. The state of the robot can be directly predicted using the motion equations of position and attitude.

[0054] 2) Further, the fusion measurement equation of inertial, visual and pseudo-satellite sensors is established, and the specific form is as follows:

[0055] Z k =h(x k )+v k (6)

[0056] in, represents the measurement value of the inertial sensor at time k, h imu (x k ) represents the measurement model that maps the robot motion state to the inertial sensor measurement space, represents the normally distributed measurement noise of the inertial sensor, represents the measurement value of the visual sensor at time k, h vis (x k ) represents the measurement model that maps the robot motion state to the visual sensor measurement space, represents the normally distributed measurement noise of the vision sensor, represents the measurement value of the pseudo-satellite sensor at time k, h psl (x k ) represents the measurement model that maps the robot motion state to the pseudo-satellite sensor measurement space, is the normally distributed measurement noise of the pseudolite sensor;

[0057] 3) Further, the motion model in step 1 is linearized to perform robot state covariance The prediction is as follows:

[0058]

[0059] Among them, P k-1 is the estimated error covariance matrix at time k-1, Q is the system process noise covariance matrix, is the estimated error covariance matrix predicted at time k, and F represents the linearized state transfer Jacobian matrix, which is in the form of:

[0060]

[0061] 4) Combine equations (6) to (8) to calculate the Kalman gain, which is as follows:

[0062]

[0063] in is a vector formed by the calculated values ​​of the inertial, visual and pseudo-satellite sensor observation models, R k is the variance-covariance matrix of the observation noise composed of multiple sensors at the current moment.

[0064] 5) Perform robot motion state correction to obtain the final fused robot state vector:

[0065]

[0066] in is the observation residual vector formed by inertial, visual and pseudo-satellite sensors, Z k It is the observation value vector formed by inertial, visual and pseudo-satellite sensors at the current moment.

[0067] 6) Calculate the noise covariance matrix of the final robot state vector:

[0068]

[0069] Where I represents the identity matrix.

[0070] After steps 1 to 6, the covariance estimate and optimal state vector estimate of the robot at the current moment can be obtained.

Claims

1. A multi-source fusion robot indoor absolute positioning method based on filtering, characterized in that: The following steps are involved: ① Select a known point in the experimental site as the initial position of the robot's movement, and deploy a pseudo-satellite base station in the experimental site to transmit satellite navigation messages; ② Install pseudo-satellite sensors, visual sensors and inertial sensors on the robot platform and calibrate them respectively; after installation, start the pseudo-satellite sensors, visual sensors and inertial sensors to control the robot to move from the initial position; ③ During the robot's movement, the processor receives the observation data sent back from the pseudo-satellite sensor, visual sensor and inertial sensor, and analyzes the observation data; ④Use the analyzed data to perform extended Kalman filtering and finally estimate the robot's fusion posture; The extended Kalman filter in step ④ specifically includes the following steps: a) Establish the robot motion state equation: x k =f(x k-1 )+w k-1 Among them, x k is the motion state of the robot at time k, x k-1 is the motion state of the robot at time k-1, f is the nonlinear conversion function from the motion state of the robot at time k-1 to the motion state at time k, and w k-1 is process noise, which conforms to normal distribution; b) Establish the measurement equation of the robot: Z k =h(x k )+v k in, represents the measurement value of the inertial sensor at time k, h imu (x k ) represents the measurement model that maps the robot motion state to the inertial sensor measurement space, represents the normally distributed measurement noise of the inertial sensor, represents the measurement value of the visual sensor at time k, h vis (x k ) represents the measurement model that maps the robot motion state to the visual sensor measurement space, represents the normally distributed measurement noise of the vision sensor, represents the measurement value of the pseudo-satellite sensor at time k, h psl (x k ) represents the measurement model that maps the robot motion state to the pseudo-satellite sensor measurement space, is the normally distributed measurement noise of the pseudolite sensor; c) Execute robot state and covariance prediction: in, is the robot motion state predicted at time k, is the estimated error covariance of the k-time forecast, P k-1 is the estimated error covariance at time k-1, f(x k-1 ) is the nonlinear conversion function from the robot motion state at time k-1 to the motion state at time k, the estimated error covariance is projected through F, Q is the process noise covariance, and F is the Jacobian matrix of f; d) Execute the correction of the robot's motion state: Where K represents the Kalman gain, H is the observation matrix, and R is the measurement covariance matrix. is the estimated error covariance predicted in step c), Z is the observation value of the robot's inertial sensor, visual sensor and pseudo-satellite sensor at time k, is the robot motion state predicted in step c), P k is the covariance matrix of the robot motion state after correction, x k It is the robot motion state after final correction.

2. The indoor absolute positioning method of multi-source fusion robot based on filtering according to claim 1 is characterized in that: The specific process of step ① is as follows: select an indoor area as the experimental site, measure a known point in the site as the initial position of the robot; deploy no less than 3 pseudo-satellite base stations in the experimental site, and the pseudo-satellite base stations are used to transmit satellite navigation messages to pseudo-satellite sensors.

3. The indoor absolute positioning method of multi-source fusion robot based on filtering according to claim 1, characterized in that: In step ②, the pseudo-satellite sensor, visual sensor and inertial sensor are installed on the robot platform and calibrated separately. The specific process is as follows: the pseudo-satellite sensor is fixed on the top of the robot body to obtain the satellite navigation message transmitted by the pseudo-satellite base station; the visual sensor is installed and fixed on the top edge of the robot body, facing the front of the robot's travel direction, and ensuring that there is no obstruction in front of the lens, so as to capture the image frame; the inertial sensor is fixed horizontally on the robot body to collect the robot's linear velocity and angular velocity information, and calibrate the relative postures of the three sensors respectively.

4. The indoor absolute positioning method of multi-source fusion robot based on filtering according to claim 1, characterized in that: The specific process of step ③ is as follows: during the robot's motion, the pseudo-satellite sensor receives the satellite navigation message transmitted by the pseudo-satellite base station, performs signal processing to obtain the longitude, latitude and altitude data in the geodetic coordinate system, the visual sensor obtains the image sequence, and the inertial sensor obtains the linear velocity and angular velocity of the image interval; The processor converts the longitude, latitude and altitude data obtained by the pseudo-satellite sensor into the UTM coordinate format and converts it into coordinate data in the world coordinate system. It solves the image sequence obtained by the visual sensor to obtain the posture change between two consecutive frames of images, and solves the linear velocity and angular velocity of the image interval obtained by the inertial sensor into quaternion, and converts it into the robot's attitude angle.