Methods and systems for simultaneous localization and calibration
Examples relate to simultaneous localization and calibration. An example implementation may involve receiving sensor data indicative of markers detected by a sensor on a vehicle located at vehicle poses within an environment, and determining a pose graph representing the vehicle poses and the markers. For instance, the pose graph may include edges associated with a cost function representing a distance measurement between matching marker detections at different vehicle poses. The distance measurement may incorporate the different vehicle poses and a sensor pose on the vehicle. The implementation may further involve determining a sensor pose transform representing the sensor pose on the vehicle that optimizes the cost function associated with the edges in the pose graph, and providing the sensor pose transform. In further examples, motion model parameters of the vehicle may be optimized as part of a graph-based system as well or instead of sensor calibration.
1. A method comprising:
receiving sensor data indicative of a plurality of markers detected by a sensor on a robot located at a plurality of robot poses within an environment;
determining a pose graph representing the plurality of robot poses and the plurality of markers, wherein the pose graph comprises a plurality of edges associated with a cost function representing an error in consecutive robot poses based on a robot motion model;
determining one or more motion model parameters for the robot that optimize the cost function associated with the plurality of edges in the pose graph; and
providing the one or more motion model parameters for the robot.
2. The method of claim 1 , wherein the one or more motion model parameters comprise a motion model parameter associated with a turning delay of the robot.
3. The method of claim 1 , wherein the one or more motion model parameters comprise a motion model parameter associated with a wheel diameter of the robot.
4. The method of claim 1 , wherein the one or more motion model parameters comprise a motion model parameter associated with a turning radius of the robot.
5. The method of claim 1 , wherein the one or more motion model parameters comprise a motion model parameter associated with a center point of rotation of the robot.
6. The method of claim 1 , wherein the one or more motion model parameters are provided to a robot control system for the robot.
7. The method of claim 1 , further comprising controlling the robot to navigate within the environment based on the determined one or more motion model parameters.
8. The method of claim 1 , wherein the pose graph further comprises a plurality of additional edges associated with an additional cost function representing a distance measurement between a matching marker detection at different robot poses, wherein the distance measurement incorporates the different robot poses and a sensor pose on the robot.
9. The method of claim 8 , further comprising determining a sensor pose transform to optimize the additional cost function associated with the plurality of additional edges.
10. The method of claim 9 , wherein the sensor pose transform and the one or more motion model parameters are determined simultaneously to optimize the cost function associated with the plurality of edges and the additional cost function associated with the plurality of additional edges in the pose graph.
11. The method of claim 9 , wherein the sensor pose transform is determined subsequent to determining the one or more motion model parameters, wherein the one or more motion model parameters are held fixed while determining the sensor pose transform.
12. The method of claim 9 , wherein the one or more motion model parameters are determined while the plurality of additional edges in the pose graph and the sensor pose transform are held fixed.
13. The method of claim 9 , further comprising alternating sensor pose calibration and motion model calibration until reaching a convergence.
14. The method of claim 1 , further comprising:
determining a second pose graph representing a plurality of subsequent robot poses and the plurality of markers based on sensor data subsequently received at the plurality of subsequent robot poses;
determining one or more second motion model parameters for the robot based on the second pose graph; and
validating the one or more motion model parameters based on the one or more second motion model parameters.
15. A robot comprising:
a sensor; and
a control system configured to:
receive sensor data indicative of a plurality of markers detected by the sensor when the robot is located at a plurality of robot poses within an environment;
determine a pose graph representing the plurality of robot poses and the plurality of markers, wherein the pose graph comprises a plurality of edges associated with a cost function representing an error in consecutive robot poses based on a robot motion model;
determine one or more motion model parameters for the robot that optimize the cost function associated with the plurality of edges in the pose graph; and
control the robot to navigate within the environment based on the determined one or more motion model parameters.
16. The robot of claim 15 , wherein the sensor comprises a two-dimensional laser scanner.
17. The robot of claim 15 , wherein the control system is configured to analyze wheel odometry of the robot to determine the error in consecutive robot poses represented by the cost function.
18. The robot of claim 15 , wherein the one or more motion model parameters comprise a motion model parameter associated with at least one of a turning delay of the robot, a wheel diameter of the robot, a turning radius of the robot, or a center point of rotation of the robot.
19. A non-transitory computer readable medium having stored therein program instructions executable by a computing system to cause the computing system to perform operations, the operations comprising:
receiving sensor data indicative of a plurality of markers detected by a sensor on a robot located at a plurality of robot poses within an environment;
determining a pose graph representing the plurality of robot poses and the plurality of markers, wherein the pose graph comprises a plurality of edges associated with a cost function representing an error in consecutive robot poses based on a robot motion model;
determining one or more motion model parameters for the robot that optimize the cost function associated with the plurality of edges in the pose graph; and
providing the one or more motion model parameters for the robot.
20. The non-transitory computer readable medium of claim 19 , wherein the operations further comprise controlling the robot based on the one or more motion model parameters.