CALIBRATION

CROSS-REFERENCE TO RELATED APPLICATION {0001] This application claims priority to LIS, Patent Application No. 15/727,726, filed October 9, 2017, w ich claims priority to U.S. Provisional Application Serial No, 62 488,639, filed on April 21» 2-017» the entire contents -of which are hereby incorporated by reference in the entireties,

BACKiMO Ni)

[0002} Commercial entities, such as manufacturers, wholesalers, and transport businesses, often use warehouses to store items, such as raw materials, parts or components, packing materials, and finished products. A warehouse can enable the organization of Hems through use of pallets and pallet racks to store numerous pallets holding various items in a manner that permits for easier access aud tlcierrl processing. As such, a warehouse niay use variou types of pallets, which are flat transport structures configure to hold items for transportation by vehicles and other equipment operating in. the warehouse.

10003} Traditionally, human operators may operate machines, vehicles, and other equipment within, the warehouse. For instance, a human operator .may navigate a forklift to lift and transport pallets between a delivery area and storage.: However, with continuous advancements in ses.vsors, computing power, and other technologies, companies are switching to autonomous and semi-autonomous vehicles for performing operations within warehouse rather than relying on uman, operators.

SUMMARY

{ 0 4] Example implementations relate to methods and systems for simultaneous localization and calibration. Mere specifically, a sensor on a vehicle may be calibrated (e.g., by determining a position and orientation of the sensor on. the vehicle) as part of a graph-based !oeaiiiiatlon and mapping system. Art example implementation may involve using sensor data indicative of multiple markets placed within an environment received detected by a sensor coupled on a vehicle located, at various, vehicle poses within the environment. The implementation may further involve deienummg a pose graph representing the various vehicle poses and the marker locations. In some examples, the pose graph may include a number of edges associated with a cost function representing a distance measurement between matching marker detections at different vehicle poses. The distance .measurement may incorporate a sensor pose on the vehicle. The implementation may then involve determining a seijsor pose transform rep esenting the sensor pose on the vehicle that O timizes i s cost function associated with the edges in the pose graph, in further examples, motion, model parameters of the vehicle may be optimised as part of a graph-based system as well or instead, of sensor calibration.

£0005) In one aspect, an example method is provided. The method may include receiving sensor data indicative of a plurality of markers detected by a sensor on a vehicle located at a plurality of vehicle poses withi an environment, and determining a pose graph representing die plurality of vehicle poses and the plurality of markers. The pose graph may include a plurality of edges associated with a cost function representing 8 distance measurement between a matching marker detection at different vehicle poses. For instance,, the distance measurement may incorporate the different vehicle poses and a sensor pose on the vehicle. The method may .further include determining a sensor pose transform, representing the sensor pose on the vehicle that o timis s the cost function associated with the plurality of edges i die pose graph, and providing the sensor pose transfomi representing the sensor pose on the vehicle.

fOCHMj In another aspect;, an example system is provided. The system may include a vehicle,, a sensor coupled to the vehicle, a computing system, a non ransitory computer readable medium, and program instructions stored on ihe non-transitory compuier readable ^• medium and executable by the computing system to receive sensor data indicative of a plurality of markers detected fay the sensor on. the vehicle located at a .plurality of vehicle poses within an environment, and to determine a pose graph representing the plurality of vehicle poses and the plurality of markers. For instance, the pose graph may include pluralit of edges associated with a cost .function representing a distance .measurement between, a matching marker detection at different vehicle poses, and the distance measurement may incorporate the different vehicle poses and a sensor pose on the vehicle. The computing .system may also determine a sensor pose■transform representing the sensor pose on the vehicle that optimizes the cost function associated with the plurality of edges in the pose graph, and provide the sensor pose transform representin the sensor pose on the vehicle.

f 00t)7j In a t irther aspect, a n.on-lransitor computer readable medium is provided.

The non-irans!tory computer readable medium has stored therein instructions executable by a computing system to cause the computin system to perform operations. The operations may include receiving, sensor data indicative of a plurality of markers detected by a sensor on a vehicle located at a plurality of vehicle poses within an environment, and detenmning a pose graph representing the plurality of vehicle- oses and the plurality of markers. For i stance, the pose graph may include a plurality of edges associated with a cost function representing a disiaiice measurement between a matching marker detection at different vehicle poses, and the distance measurement may incorporate- the different vehicle poses and a sensor pose on the vehicle. The operations may further include determining a sensor pose transform representing the sensor pose ΟΏ the vehicle that optimizes the cost function associated with the plurality of edges mi .pose-graph, and providing the sensor pose transform representing the sensor pose on the vehicle.

{0008] in yet another aspect., a system s provided . The system may include means for loea!i¾ation and calibration, in particular, the system, may Include means for receiving senso data indicative of a plurality of markers detected by a sensor o a vehicle located, at a plurality of vehicle poses within an environment, and means for determining a pose grap representing the plurality of vehicle poses and the plurality of markers, hi some instances, the pos grap may a plurality of edges associated with a cost function representing a distanc measurement between -a matching marker detection at different vehicle poses, and the distance measurement may incorporate the different vehicle poses and a sensor pose on the vehicle. The syste may also include means- for determining- a sensor pose transform representing the sensor pose on the vehicle that optimizes the cost function associated with the/plurality of edges in .the pose graph, and means for providing the sensor pose ^' transform representing, the sensor pose on the vehicle.

fOoWj These as well as other aspects, advantages, and alternatives will become apparent to those of ordinary skill in the art by reading the following detailed -description, with reference where appropriate to the accompanying drawings.

BRIEF BESC PTI0N OF THE .D.RAW3&GS

(0010} Figure Ϊ is a block diagram of a system, in accordance with an example embodiment,

{0011] Figure 2 depicts a system, for operating one or more warehouses, in accordance with, an example embodiment..

|O012| Figure 3 illustrates a system, in accordance with an example embodiment.

{0013] Figure 4 illustrates robotic device architecture for one or more robotic devices, in accordance with an example .embodiment

00.1.4} Figure 5 illustrates laser scanner architecture for one or more robotic devices, i accordance with an example embodiment,

{001 §| Figure 6 shows a device lor pre-mapping a space, according to an example e bodiment

[0M6J Figure 7 shows a robot navigating wiilijs an environment, according to an example embodiment

|0Oi7] Figure 8 shows a robot associating detections - ith mapped landmarks, according to an example embodiment

{0018} Figure 9 is a functional, block diagram illustrating modules of a robot control system, according to an example embodiment

|0019| Figure 10 is a .flowchart thai shows aft example method for localization and calibration.

{0020} Figure 1 ],A illustrates a pose graph, in accordance: with example em odiments.

{0021} Figure UB illustrates a pose graph that incorporates motion model parameters, in accordance with example embodiments.

DETAILED JOESCIUPTIO

|0 22j Example methods and systems are described herein. It should be understood thai die words, " am le," "e^emplat , ^** and "illustrative" are used herein to mean "serving a an example, instance, or illustration.' ^* Any implementation or feature -described herein as being art "example," being "exemplary,'' o ^' being "illustrative" is riot necessarily to be construed s preferred or advantageous over other implementations or features.

0023} The example implementations described herein are not meant to be limiting. It will be readil understood that aspects of the present disclosure, as generally described herein, and illustrated in tire figures, can be arranged, substituted, combined, separated, and. designed in a wide variety of different configurations, all. of which are explicitly con ie.mpiai.ed herein.

I _* Cteerylew

{0024} Advancements in computing and sensor capabilities have helped contribute to an increase in the deployment of robotic devices (robots) to perform operations within warehouses and other types o f en vironments. Although some opera tions can be performed by stationary robots, man tasks often requite a robot to successfull navigate between multiple positions. Therefore,, in. order to complete tasks, a mobile robot may require a substantial miders¾nding o f the environment

0025} In practice, various methods may be used to develop information for a mobile robot to use to navigate an en ironment effectively. Particularly, a mobile robot ma rely upo one or more maps of the space that can enable its control system, to determine proper navigation strategies around physical boundaries. {00263 to- develop a m&p of an. en ironment, ail example method may involve

.manually surveyin the environment to determine the positions of various landmarks that a robot may use . during navigation. ^' Landmarks represent .. detectable features in the environmen that can be used for position and orientation reference. For example, a warehouse may include rorefieciive -.markers (markers) or other infrastructure positioned at heights detectable by sensors on robots and at particular locations to help . uide fee robots. Markers and other pos ible landmarks are useful since they can be re-observed from different positions and different angles as a robot changes position. For illustration purposes, markers will b used as the -primary land rks discussed herein, but other types of landmarks are also possible withi examples.

{0027] Although a manual survey of an environment may enable the creation of an accurate map of the markers that a robot can use, this process is time consuming and can. delay the deployment of robots. The method may also require, subsequent tests to identify particular areas that prove difficult to navigate without adding more markers,.

{00281 In order to speed up the -mapping process and enable real-time user feedback, computing system may perform a simultaneous localization and mappin (SLAM.) process to build a map of an unknown environment (e.g., a warehouse) using measurements provided by a sensor coupled to a robot while the robot also navigates the .environment using the map. SLAM may involve marker detection, data association, pose estimation, and pose/marker refinement, and can be performed in either two-dimensions (2D) or three-dimensions (3D) using a variety of sensor data, such as laser scans of the environment. While erformi SLAM, the computing system may develop a map that specifies positions of detected, markers that can he used by the robot and. other devices deployed in the environment.

10029] The performance of SLAM to localize a robot within an environment is often performed relative to the position of a sensor on the robot. As a result, SLAM may produce sequences of sensor poses and a map of detected markers relative to the position and orientation of the sensor in. the environment, not the robot. To account for differences between the sensor poses as indicated by SLAM: and the poses of the robot within the environment, fixed sensor-to-robot transform ma be used, to localize the robot in. the map if needed, For instance, the fixed sensor-to-robot transform may be used for navigating the robot within the environment

10030) In some instances, the fixed sensor-to-robot transform, may produce errors that impact localization of the robot within an environment. For .instance, the position of the sensor on the robot can change over time causing an increase in the difference between robot poses determined -usmg the fixed ensctf-to-rohot -_ra«s.fori« and the actual poses of a robot Av thiii the environment In other examples, a fixed sensor-to-robot transform may be consistently inaccurate- by the same amonnt due to a mounting ^' error of the sensor on the robot. The above situations as well as others may yield errors that reduce the ability for the robot io navigate the environment accurately.

{0031} Example implementations & ^Γ localization and. calibration may help reduce error's that can arise from reliance upon a fixed sensor-to-robot transform. Particularly, rather than relying upon a fixed sensor-to-tobot transform, when localizing a robot within an environment, a computing system may determine a pose of the sensor (sensor pose) relative to the robot to reduce potential errors.. For instance,, the computing system may determine a sensor pose that optimizes a cos function such thai the sensor pose produces minimal error. The computing system may also determine and use the sensor pose to identify when the sensor pose differs . from prior expectations. This enables the computing system to account for an identified difference in the sensor pose when deterimning robot poses within th environment

CHXS:2J in some examples, when optimizing- a cost ftmction to determine the sensor pose, the cost function may relate detections of the same marker at different robot poses. For instance, a computing system may receive sensor data that indicates positions of markers within ^' the environment The sensor data received at the computing system ma be from a sensor coupled to a robot when the robot is located at various poses within the environment. As such, ^' the- sensor data may capture markets positioned within the environment from different angles as the robot changes position and orientation.

& 33J The computing system may use incoming sensor data and the various robot pose to determine a pose graph that represents the robot poses and the detected markers. In particular, the pose graph may include a number of edges associated with a cost function representing a distance measurement between one or more matching marker detections - at different robot poses. The distance measurement may incorporate the different robot poses and a sensor pose on the robot. The computing system may determine a sensor pose transform that represents the sensor pose on the robot which, optimizes the cost function associate -with the edges in the pose graph, and also provide the sensor pose transform representing the sensor pose on the robot. For instance, the computing system may provide the sensor pose transform to- the robot, control system for use during navigation.

|0034] further examples, a cost function may relate detections of one or more marker to mapped positions of the markers when a prior map is available. For instance, the computing system may determine a sensor pose ^■■ transform to o timise the cost function associated wit edges in the pose graph thai represent a distance between a marker detection and a previousl mapped position -of the marker. Such edges may be included n the pose graph in addition to ot instead, of the edges relating distances between matching marke detections at different robot poses. When, a prior map of markers is available, a robot may ^' be controlled to make a series of movements relative to the mapped markets as part of a ealibmtion process to determine the sensor pose.

{003S] lit some examples, the computing system ma simultaneously determine th seasor pose transform a d the various robot. oses relative, to the map of markers in order to Optimise the cost function associated with the edges in the pose graph. For instance, the computing system may determine the sensor pose transform as the tobot navigates - within the: environment incorporating both new measurements and. new robot poses when determining the sensor pose transform. ^' The new measurements and robot poses may also be used, to ^■ adjust the sensor pose transform. Accordingly, the sensor pose transform may be continuously or periodically updated using new information to minimize potential errors. |M36| in further examples, the computing system ma determine the sensor pose transform subsequent to. determining robot poses within the environment, f or instance, the sensor pose transform- may be determined when the robot is offline (i.e., during non- operation period). When determining the sensor pose transform after determining robot poses within the environment, the computing system may hold the robot poses as fixed while ^' determining the sensor pose transform,

} 5fj In some examples, a computin system may perform multiple iterations of SLAM to verify the accuracy of a determined sensor pose transform. In some eases, the computing system may determine that the sensor pose transform requires modification. As an example, after determining an initial pose graph, the computing system ma determine an additional pose graph representing subsequent robot poses and detect markers. The additional pose graph may be based on sensor data subsequently received when the robot is located at the subsequent robot poses. The computing- system may then determine a second sensor pose transform based on the additional pose graph and use the second sensor pose transform to check, the accuracy of the initial sensor pose transform, Th computing system may repeat this proces to validate a previously determined sensor pose.

0038] In some instances, the second sensor pose transform may validate and confirm: that the initial sensor pose transform does not require Modification. Particularly, a match between a previously determined sensor pose transform and the subsequently determined sensor pose transform a serve as- coBfttiaaiioti that the prior sens r pose- transform is aceuKVte, hi other instances, however, a subsequent pose transform may differ from prior sensor pose transform. If this is the case, the computing system may identify a difference between the initial sensor pose irasistorm mid the subsequent sensor pose transfo and provide a -calibration error signal that specifies the identified difference between the initial sensor pose transform and the subsequent pose transform _* The computing system may repeat this process using additionally deternhned sensor pose transforms to mrther calibrate prior sensor pose transforms.

|003:9j hi additional examples, a cQmpot ng system, may use pose graph to determine the sensor pose of multiple sensors positions on the robot.. For instance, the computing system may determine a sensor pose for a first laser scanner and a second senso pose for a camera positioned op the robot. The sensors may be the same or different types. { ^' 06 0] When detern uing additional ^' sensor poses _* the computing system use additional sensor data from other sensors to determine the sensor poses for those sensors. For instance, the competing may receive sensor data indicati ve of markers from, a second sensor on the robot when the robot is located at the various robot poses within the environment as specified in the pose graph. The computing system ma modify the pose graph to include additional edges based on the sensor data from the second sensor arid a sensor pose of the second sensor on the robot. As a result, the computing system may use the pose graph to determine- a second sensor pose transform representing the pose of the second sensor on the robot. The computing system may also provide the sensor pose trans orm for the second sensor to the robot control system or other computing systems. In a further example, the computing system may repeat th above process for additional sensors positioned on a robot. |00 1 j In some examples, -a pose- _. graph may be also used to optimize one or more motion model parameters o a vehicle. Motion model parameters ma be optimized in addition to or instead of sensor calibration. A computing system, may use edges in the pose graph that rela e only to the robot pose and not rely upon detected markers when, determining motion model parameters, Partieoiariy _* the computing system may compare consecutive robot poses to predicted motions of the robot based on motion model parameters. If the .motion model parameter are inaccurate, the estimated vehicle motion may systematicall deviate from actual vehicle motion. For instance, the -computing sysiem ma anal ze wheel od-omeiry, time between scans, control instractions, and/or other factors when determining the robot motions. The computing system, may etermine the pose graph to include additional edges; associated with an additional cost fiinction representing an error in consecutive robot poses based an & robot motion model,

[0042] The computing system may then determine one at mate motion model parameters for the robot that optimise the additional cost function associated with the additional edges in the pose graph. For example, motion model parameters may sBclq.d a turning delay, a wheel diameter, a turning radius, and a center point of rotation. In some instances, the computing system may hold the edges in the pose graph and the sensor pose transform fixed when determining the one or more motion mode! parameters. The motion model parameters) for the robot may fee provided to die robot control system or other computing systems.

|0fl43] Sensor pose calibration and motion model calibration may be run Separately or together (e.g., alternating sensor pose calibration and motion model calibration),. Is further examples, the computing system may update one or .more of the robo t pose in. the pose graph based on the one or more motio model parameters,. The computing system may also subsequently adjust the sensor pose- transform after updating one or more robot poses in the pos graph,

II. System Design for Robotic Devices

fOQ43j Figure 1 is a Mock diagram of system 100, in accordance with an. example embodiment. System. 100 includes planning system 110 and robotic device Ϊ20, Planning syste 1 10 can. include oflboard planner 1 12 thai can coordinate motions of one or snore robotic devices operating in an environment. Offboard. planner 1 12 may include roadma .planner 114 such that offboard planner 112 arid/or roadmap planner 114 may generate one or more - asynchronous paths 116 for a robotic device (e.g., robotic device 120) to follow in an environment

f0044] A roadma graph, prototype graph, or other roadmap -representative of an environment, such as prototype graph 300 discussed below in tire context of Figure 3, can be received, determined, or otherwise provided to planning system 110, oflboard planner 112 and/or roadmap planner 1 14. Asynchronous paths 116 can be one or more paths developed based, on one or more of the roadmap graph, prototype graph, or other roadmap, For example, if the roadmap graph, prototype graph, or other roadmap has a plurality of edges that connect a plurality of intersections, .asynchronous pa hs 116 can be specified in terms of the plurality of edges arid/or the plurality of intersections ,

|O045] In some examples,, robotic device 120 can be any one or more steered vehicleCs) capable of .following a path. For example, robotic device 120 can include .onboard, software 130 and/or hardware 150. Onboard, -software 130 c r include one or more of: locahmiion subsystem 132,, obstacle detection subsystem 134, odornetry subsystem 136, aiti-fbS lowing subsystem, 138. and trajectory-following swbsyslsm 142, As such, onboard software 130 may isiclitde additional software systems m other examples.

0O46] Localization subsystem 132 represents a system capable of localizing a robotic device, in other words, localization subsystem 132 may enable location determination of the robotic device with respect io an environment For instance, localization subsystem i 32 can generat ^' osition estimates of the robotic device and/or other objects that can be used to localize the robotic devic and assist the robotic device in following a desired path (e.g., asynchronous paths I !£>}, and/or assist the robotic device in following a trajectory (e.g,, trajectories 140), Once the position estimates are generated, localization subs stem 132 can provide the position estimates to path-folloxving subsystem 13 .

(0047} An asynchronous path,, or path for short, can be a time-invariant plan or other informatiori indicating how robotic device 120 ma travel from a starting point (SP) to m ending point (EP); i. e. , an (asynchronous) path does not take time into accoun In contrast, a trajectory can include Values of a steering angle and of traction motor velocity that robotic device 120 can follow for a planning time interval

fOtWSj The planning time interval can be a duration of time during which a robotic device is guided, or planned, to follow a path, route, and/or travel, 1» some embodiments, the planning time interval -can be a predetermined amount of time; e.g., five seconds, one second, 0.2 .seconds, 0.1 seconds. In particular, a predetermined planning time interval -can fee determined based on a user nput that specifies a value for tire planning time interval. In other embodiments, the planning time interval can be determined based on one or more other values- e.g., a stitch time, a time associated with a uniform edge (or path) cost, an estimated time to travel along a trajectory. Other techniques for determining the planning time interval and values for the planning time interval are possible as well.

(0049) Then, one or more trajectories can he used io describe how robotic device 120 can travel from starting point (SP) to an ending point (EP) in a time-variant manner. In some embodiments, a trajectory can also: provide information about values of other variables than a steering angle and a traction, motor velocity over the planning time interval, such as, but not limited to, other kinematic variables (e.g., velocity and acceleration) of robotic device 120, and actuator positions of robotic device 120,

(0050 j As an example, a path to drive a car from a location "home" to a location

"work" may include an ordered listing of streets that a control entity, such as a person or control device of an autonomous vehicle, can use to drive the car from home to work, in mis ex mple, a trajectory from home to work can involve one or more instructions specifying velocity and/or acceleration that the control entity can use to drive the ear -from, home to work. In some examples, the trajectory can take traffic, obstacles, weather, and other time- ^• sensitive conditions into account; e.g., the trajectory to go from home to work can in cate that the control entity "tarn right for M) seconds at 20 MPH or less", "accelerate to 55 MPH and drive strai ht for 3 minutes", "slow to 20 MPH within .30 seconds", "turn left for 1.0 seconds at 20 MPH or less", etc. in some embodiments, the trajectory can be changed along the way; e.g., to account for obstacles, changes in path, etc,

|j9051] Obstacle deteoiiou subsystem 134 ca determine whether one or more obstacles are blocking a path and/or a trajectory of robotic device 120. Examples of these obstacles can include,, but are not limited to, pallets, objects that may have fallen off a pallet, robotic devices, and human operators working in the environment If an obstacle is detected, obstacle detectio subsystem 134 can provide one or more communications indicating obstacle detection to path-following subsystem 138.. The one or more communications indicating obstacle detection can include location information about one or more positions of one or more obstacles detected by obstacle detection subsystem 134 and/or identification inforai tion. about the one or .more obstacles detected b obstacle detection subsystem 34, Odometry subsystem 136 can use data, such as data from servo drives 152, to estimate one or more changes in position of robotic device 120 over lime.

{0052] Path-following subsystem 138 and/or trajectory-foiSowin s bsystem 1 2 can act as a planner aboard robotic device 120. This onboard planner can follow one or more paths, such as asynchronous paths 11.6, based on position estimates _. provided by localization subsystem 132..

10053] Path-following subsystem 1,38 cars, receive asynchronous paths 1. 16, position estimate inputs from localization subsystem 132, location information about one or more positions of one or more obstacles .from obstacle detection subsystem. 1.34, and or infortnstion about one or more changes in position from odometry subsystem. 136; and generate one or more trajectories 140 as outputs..

0054] Hardware 150 can include servo drives 1.52 and or ^' motoss 154, Servo drives

152 can include one o m r servomechanisms and related electrical equipment. In some examples, servo drives 152 can include one or more electronic amplifiers used to power tie one or more servomechanisms and/or to monitor feedback signals from the servoraectanismCs), Servo drives 152 can receive control signals, such as trajectories 144, from onboard^ software 130, and can provide electric current to the servomechauism(s) to produce .mot on proportional to file control signals. In. some embodiments, servo drives 152 can compare status information received, from, the servomechaat$.m{s) with an expected status as commanded by trajectories 144, Then, servo drives 152 can adjust a voltage frequency or pulse width of the provided electric current t correct for deviations between received -states information and w expected states, in other embodiments, servo drives 152 can provide information, such as the feedback signals, and/or location-related information, to onboard, software 130.

fOOSSj One or more -motors 154 can be part or all of the ^• servomeehani-sntCis ^' ) of servo

-drives 152. For example, motors 154 can use the electric current provided by servo drives 15 to generate mechanical force to drive part or all of robotic device 1 0; motors 154 can provide force to propel robotic device 120 and/or drive ne or more effectors of robotic device .120.

10056] Path planning of robotic devices within an environment, such as an environment that includes indoor settings, such as a warehouse, office building, or home, aad or outdoor settings, such as a park, parking lot, or yard, can be performed with, respect to a roadmap graph, which is a connected graph of paths t t agents, such as robotic devices, may follow. Using road ap graphs to plan agent routing within the environment rather than taking a free-space approach can reduce total planning state space and so making large- scale mufti agent coordination tractable, Further, the use of roadman; graphs can enable operators to intuitively control areas in. which robotic devices are allowed to navigate, f 0057j Roadmap graph generation can llrst involve generation of a prototype graph, which indicates the rough position of lanes and directions of travel, hi some examples, prototype, graph can be a directed graph that .indicates lanes and directions of travel of robotic devices. In other examples, a. prototype graph can be generated manually based on a map or drawing of the environment.

JOOSS] Figure 2 depicts system; 200 for operating one or more warehouses, in. accordance with an example embodiment System 200 includes warehouse management system. 1Θ, planning system 110, and robotic device 220. Warehouse management system 21 can receive one or more logistics requests 212 associated with the warehouse; e.g, requests to store one or more items hi the warehouse and/or requests to ship one or more items from the warehouse:. Warehouse management system 210 can translate logistics requests. 212 into one or more actions 214, where actions 21 can Include, but are not limited to, a- "iftove-to" action to move one or more designated agents to one or more designated locations, and a "transport" action to carry one or more items to one or more designated locations, in some examples, actions ^' 2.14 can include go-to commands of the form {agent ID, destination], but other actions are possible such as "move pallet". These are typically decomposable into move-to commands, howeve (move to pick location* move to place location}.

{0059] Planning system 110 includes ofiboard planner 1 12 and executor 220.

Ofiboard planner 112 can receive aciions 214 as inputs and generate one or more coordinated paths 216 for one or more agents operating in a warehouse; e.g., multiple robotic devices, to cany out actions 214, Coordinated- paths 216 can be pari, of a .coordinated action plan for all agents irt the wareliou.se to fulfill logistics retpests 212.. The coordinated action plan can. take precedence of agents into account; .e.g. , if robotic devices RDl and S.D2 are both expected to reach a point at approximately the same time, one of the robotic devices ean have precedence of .priority over the other, sach as robotic device RBI wailing for robotic device RD2 to pass through the point (or vice versa), Executor 220 can receive coordinated ^' paths 216 and generate non-conflicting sub-paths 222 to direct robotic device 120 in accomplishing its part, of the coordinated actio plan to carry out actions 214 to fulfill logistics requests 212.

f.(MK> .j A illustrated above in Figure 2, planning system _. 110, which includes offboarcl planner 112 and executor 220, can communicate with robotic device 120. ^' In some embodiments, the robotic device ean be a fork -truck; for example, any Occupational ^' Safety and Health Administration (OSHA) Class 1 or Class 3 powered industrial truck, in other embodiments, planning system 1 .10 ca includes software that executes using one or more networked computing devices located in the "cloud" (e.g., one or- more networked ^' computing, devices) and/or located somewhere on a premises co-located with robotic device 120.

( _. 0061 j Figure 3 illustrates a system 300 that includes logistics interface 310» warehouse management system 210. and one or more robotic devices 120 connected using network 318, in accordance with an example embodiment. Logistics, interface 310 can provide inventor task instructions io warehouse management system 210 via network 318 regarding movement of objects, such as pallets, and or robotic devices to warehouse management system 210, A example inventory task can be to move pallet A containing items of type. B to location C.

|0062j Warehouse management system .210 can receive the inventory task instructions from logistics interface 310 -and generate one or more task / mission instru.cli.ons (e.&, art instruction to robotic device A to move pallet B from location C to location D) and/or plans for controlling robotic device(s) 20 to carry out die inventor}-' task instmetions. The task mission instructions ^' and/or plans can include information about one or more paths ^• and/or one or more trajectories, where the task mission insirueOonis), pran(s), path(s) and t jecior /trajectories. are generated by planning system 11.0 of ware ouse management system 210 using the techniques discussed in the context of Figures 1 and 2,

|0O63] For example, warehouse management system 210 can be a centralized control service running on and storing dat using one or more computing devices; e.g., ser er eompiuing devices. To perform these tasks, warehouse management system , 210 can include WMS middleware arid can provide a use interface -to provide access to tools for monitoring and .managing system 300. The WMS .middleware and/or other components of warehouse management system. 210 can use one Or more application r g ammng interfaces (APIs), such as protocol conversion APIs lor conversion between task / mission Instructions ( .g., an instruction to robotic device A. to move pallet S from location€ to location D) to robotic device paths, poses, and/or trajectories; conversion between nventory tasks and task mission instructions; and conversions between APIs.

|006 | The user interface provided by warehouse management system 210 can provide one or more user interface functions for .system 300, including, hut: not limited to: monitoring of robotic deviee(s) .120, e.g, presenting data related to location, battery status, state of charge, etc, of one or more robotic devices; enabling generation and sending of inventory task instrUction(s} ₅ task / mission instructionCs), plant ^' s), path(s) and or trajectory/tmjeeteries to one or more of robotic deviee(s) 120; and reviewing, updating, deletion, am! or insertion of data related to one or more warehouse maps, pallets, networks, and/or planning systems (e.g. _f planning system l it), warehouse m agem n system 210, and or logistics interlace 310),

006δ| in some embodiments,, warehouse management system 210 can. route coramunicsijons between logistics interface 310 and robotic deviee s) 120 and between two or more of robotic device(s) 120 and manage one or more onboard systems, such as onboard system 320 aboard one or more of robotic deviee(s) 120. In other embodiments, warehouse management system 25 can store, generate, read, write, update, and/or delete data related ^' to system 300, such as _* but not limited to: data regarding completion of a task / mission instruction by one or more of robotic device(s) 120: data regarding, locations and/or poses of some or all. of robotic d.evice(s) 120, including data indicating a location where a robotic device was initialized / booted; data related to one or more audit trails for human actions, incident analysis, and/or debugging; and data for state tracking. In other embodiments, warehouse management system 210 can include a central message router/persisience manager that communicates with robotic devtce(s) 120 and one or more adapters. Each of the one or more adapters can provide access to data and/or ce miaiicatiorss of system 300 available to warehouse management system 210, and can include, but are not limited, to: a user interface service adapter for die above-mentioned user interface*, a web content service adapter enabling World Wide Web (WWW) / Internet access to informatics about system 360, a message proxy adapter and/or a WMS- adapter to act as intermediaries- between communications between APIs and/or the WMS.

[0066] Figure 3 ^' shows that each. of the one or more robotic devices 120 can include one or more of; onboard system 320, .network: ^' switch. 330, vehicle controller 332, programrnabSe logic controller (PLC) 334, one or more device sensors 338, and one or more drives 340,

£0067] Onboard system 320 can be a computation and sensor package for robotic planning configured fo installation, into and use with robotic device 1.20, - where onboard system. 320 can include onboard sensors 322 and one or more planning/execution processors 324. Figure 3 also shows thai onboard system 320 thai is configured to use network switch 33§ at least to communicate with plannin system 110 (via network 318), with device sensors 338, and¾r with, one or more actuators of robotic device 120.

|0068| Onboard system 320 can be responsible for one or more of; localization of robotic device 120, generation of local trajectories to carr out plans and/or travel along paths and/or trajectories provided by warehouse management system 210, generation of commands to dri es 340 to follow one or more (local) trajectories, ^' generation of commands to control actuators) of robotic device 120, and reportin pose, status and/or other information to warehouse management system .210.

[006*?] Onboard sensors 322 can include one or more navigation lasers, laser scanners, cameras, and/or other sensors for navigating and/or controlling onboard system 320, For example, a robotic device of robotic devieefs) 120 can include one or more laser scanners (e.g., laser scanner in Figure 5), such as one or more laser scanners provided by SICK AG of aldkirch, Germany, HOKIJYO AUTOMATIC CO. LTD of Osaka, Japan, and/or KEYENCE CORPORATION of Osaka, Japan. The laser scanners can be used for obstacle detection and/or avoidance along a direction of travel of the robotic device as well as along the sides, corners, and/or back of the robotic device. The laser scanners can also be used, io localise the robotic device using reflector-based localisation. In- some embodiments, cameras and or other sensor can be used for obstacle detection, obstacle avoidance, and/or localization instead of or. alon ilh the laser scanners,

{0070] Planning execution _. processor(s) 324 can include one or more computer processors- connected at least to onboard sensors 322. Planning/exeontiott processor(s) 324 can read data from, onboard sensors 322 _s generate local trajectories -and/or commands- to drive(s) 340 to move robotic device 120, and communicat with warehouse management system 210, A local trajectory can be a trajectory where robotic device 120 starts at a starting pose arid reaches an. ending pose at some time. In some examples, the starting pose can be implicitly- specified; the starting pose can be a CBrren o.se of robotic device 120 and. so the local trajectory be base on an assumption mat its startin pose is the current pose of robotic device 120.

|0 71j PlanBmg/exeeoiion. proeess r(s) 324 can utilize a CompOiie¾i:¾mework, The component framework can be a multi-threaded job scheduling and message passing system built on software libraries for input/output (I O) and signaling configured, to provide a- consistent asynchronous .model of robotic device: 120, such as the "boost: :asio" and. "boost; ; signals!" software libraries provided by boost.org of Onaocoek, Virginia. The -component framework- can esabie conimiinication between software components (or modules) so that the ^' software components can be executed in parallel in a thread safe manner. fO-072] The component framework can. include one or more of: a state machine component, a localization component, a planning component, and a trajectory following component. The state machine component can manage state of robotic device 1 0 lor vehicle imtiaikatjon, vehicle commanding and fault handling. The state machine component can .use -a deterministic finite automaton or other state machine to manage the state of the robotic device.

f¾§73| The localization component can read data from, vehicle sensors- and integrate prior state information of robotic device 120 to determine a pose of robotic device 120,. The vehicle sensor data may be indicative of one or mors laodmarks/points o interest detected by the vehicle sensors. Alternatively, the data from the vehicle sensors may require processing such that the localization component detects the one or more landmarks/points of interest based on the vehicle sensor data. The pose can be determined relative to the one or more detected landmarks/points of interest, such as pallets or other objects. The planning component can receive one or more objectives .from warehouse management system 210 and determine a local trajectory for robotic device 120 to achieve those objectives,. In some embodiments, the local trajectory can be a short-term trajectory that robotic device 120 is to follow for a predetermined amount of time; e.g., 100 milliseconds, 200 milliseconds, 500 milliseconds, .1 second, 5 seconds. The trajectory following component can receive the local trajectory generated by the planning component, and generate drive control instructions to travel along the local trajectory. The drive control instructions that are then relayed to drives 340 that control a traction, motor and other actuators far robotic device 120.

[00 ^* 74] Network ^■ switch 330 can enable ^■ ■communications for robotic device(s) 120,

These communications can include, but are not limited to, communications between onboard system 320 and the rest of robotic device 120; e.g, device sensors 338 and drives 340, and. communications with warehouse management system 210 via network 318, For example, network, switch 330 can enable Transmission Control Protocol internet ^{" '} Protocol (TCP iP)- based communications over Ethernet and or other wireline conuMffiieatkms interiaee(s) to a wireline: .network arid/or over Wi-Fi*** arid/or other wireless communications in erfaceis) to a wireless network, such as a PLANET Ethernet: Switch by PL A E Technology Corporation of New Taipei City, Taiwan.

fOOTSJ fa some embodiments, communications between robotic deviceis) 120 and planning system 1. 10 cars include remote procedure calls (RPCsL The remote procedure calls can allow invocation of software procedures, methods, and/or functions resident, on one or more of robotic device(s) 120 by software of planning system. 1 10 and. vice versa. The remote procedure calls can. be based on a communications protocol such as TCP/IP, a HyperTexi Transfer Protocol (HTTP) .such, as HTTP 1.0 and/or HTT 2,0, and/or another communications protocol Some or all of the remote procedure calls can include encrypted data; such data .may be encrypted using the Secure Sockets Layer (SSL), Transport Layer Security (TLS), and/or one or more other encryption algorithms and/or protocols.:. In. embodiments where encrypted data is used _* one or more certification authorities, such as a private certification authority, can authenticate one or more certificates used, in encrypting, and/or decrypting the encrypted data, A certificate authority can use an access control list (ACL) to control access to the one or more certificates. The remote procedure calls can use a request response protocol and/or a bidirectional streaming protocol for RFC-related communications. In embodiments where the bidirectional streaming protocol is used fo RPC-re!ated -communications _* a single long-lived RFC can be used to implement the bidirectional streaming protocol.

[0076] Vehicle controller 332 and/or programmable logic controller 334 can provide electrical and sensor .management functionality for robotic. device(si 120. _f The electrical and sensor management funciiooahtv can include, but is not limited to, functionality for electrical load control, lighting, control, sensor control sensor and/or switch signal processing, and power management. Vehicle master 336 can provide tuneuonahiy tor controlling One Or more actuators, such as lift devices, of robotic device(s) 320,. {00773 Device sensdr(s) 338 can Include one or more sensors t¾at can provide data related to controlling and/or operating robotic deviee(s) 120, The data can provide information about an emironme about robotic device(s) 120, such as but not limited, to, localization information _* positionestitnates, an mapping data. For example, device se&sorcs) 338 can include one or more lasers (e.g.. two-dimermonal (2D) lasers, .safety lasers, laser scanners), cameras (e.g., Time-of-fiight (JoF) cameras _* Red-Green-Blue (RGB) cameras, thermal cameras), electrical sensors, proximity ensors, navigationa.! devices, and location sensors.

{0078] Drivel ^' s) 340 can include one or more drive controllers and/or actuators thai provide ftinctionaiity for moving robotic device(s) 120, toe drive controllers can direct the drive ac tuators to control movemen -of robotic deviceis) 120, The drive actuators can include one or ore traction. motors, electric drives, hydraulic, drives, and. pneumatic dri ves.

f007<?| Figure 4 illustrates robotic- devi ce architecture 400 of robotic device! s) 1,20, in accordance with an example embodiment Robotic device architecture 400 of robotic devrce(s) 120 can include software. The software can include software for localization 410, software to a pallet pose estimator 412, software related to state machine 414, software for planner follower 416, soft r for componen framework 420 and software for operating, system 430, The software can be executed by one or more hardware planning/execuiioB processors 324, .Communications between robotic device(s) 120 and other devices can be carried out using network gateway 440 and/or network switch 330. For example, network gateway 440 can be used for wireless communications, wit and within a robotic device of robotic device(s) 1.20 and network switch 330 can be used for wireline communications with and within, a. robotic device of robotic device(s) 120. Robotic devic architecture 400 also includes additional hardware such as device sensor(s) 338 and drivefs) 344) discussed above i the context of Figure 3, in some embodiments, robotic device architecture 400 can include one or more cameras, including but not imited to, ToF camera 450 and RGB camera 452, where the one or more cameras ca include one or more still cameras and/or one or more video cameras,

008¾J Figure 5 illustrates, laser scanner architecture 500 for robotic device(s) 120, in accordance with as example embodiment- Ϊ» some embodiments, some or all of device sensorCs) 338 can be lasers and laser scanners illustrated by laser scanner architecture 500. 10081} Laser scanner architecture 59ft can include lasers 510, 512, 529, 522, laser scanner 524, protocol converter 526, network switch 330, and onboard system 320, Lasers 510, SI 2, 520, and 522 can. be located at fixed positions of robotic deviceis) 120; for example, laser 510 cm be located at the front of a robotic device, laser 512 can be located at the rear of the robotic device, laser 520 can be located at a front left corner of the robotic device _s -and laser 522 can be located at a front right corner of the robotic device. Lasers 510, 512, 520, 522, jwid or laser scanner 524 cm provide information to localize the robotic device withi an environment. In some embodiments, lasers 510, 512, 520, 22, and/or laser scanner 524 can emit light thai is reflected off of one or more reflectors— fee reflected light can be detected fay one or more laser sensors, and the robotic device can be localized within the eiiviroftrnerit based on a duration of time take to detect ttie reflected light, in particular of these eiriijodirnenlS:, some or ail of lasers Sir), 512, 520, 522, and/or laser scanner 524 can include: one or more laser sensors for detecting reiected laser light. Then some: or all of lasers 510, 512, 520, 522, and/or laser scanner 524 can generate data, including but not limited to, data related to a laser (e.g., maintenance data for ihe laser), data related to light emitted by the laser, and data related to one or more derations of time taken to detect reflected lase light fa the laser sensor(s).

|O08 j As illustrated in Figur 5, some lasers, such as lasers 520, 522, and laser scanner S24 can be directl connected to network switch.330, while other lasers, such as lasers 510, 512, can- be connected to network switch 330 via protocol converter 526. Protocol converter 526 can convert a communications protocol used by a laser, such as -laser 510 and/or 512, to a communications protocol used by network switch 330; e.g„ convert from a -communications .protocol based on S-422 to a communications protocol based on Ethernet. Then, lasers 510, 512, 520, 522, and laser scanner 524 can send data to and receive commands from onboard system. 320 via network switch 330 and perhaps protocol converter

10083] in some embodiments, robotic device(s) 120 can be subject to one or more failure conditions. Examples of those failure conditions and related recovery strategies are described in Table 1 below.

Trajectory Following Trajectory following error Robotic device will halt and exceeds ttitesho.ld. attempt t restart: trajectory following automatically. If system fails twice in a row then huraan. operator will be notified. The operator can intervene by manuall driving robotic device back onto roadmap..

No Safe Trajectory Due to obstacle -proximity _* Rohottc device will halt and the tnyeetory lanner cannot notify human operator. The Bad a safe .trajectory that operator can intervene by would keep the robotic manually driving robotie device device a safe distance from around obstacle.

.known obstacles.

Hardware Fault Steering/traction drive fatilt Robotic device will halt and or oilier low-level hardware notify human operator. The. I/O operator can power-cycle and fault cond tion manually drive robotic device back onto roadrnap.

Pallet Detection Failure Robotic device expected to Robotie device will send message discover a pallet at to a control service that Includes ■commanded location; no sensor data relative to where the pallet, was found pallet was expected to be discovered. The control service will notif human operator and optionally may send pallet pose information manually.

Pallet Pose Estimation Robotic device could not Robotic device will send message Failure determine pose of pallet to a control service that includes relative to robotic device at sensor data relative to where the high confidence; pallet was expected, The control

Service will notify human operator and send pallet pose information manually.

Table J

IIL Example Fre-mappmsi an Environment

{0084] Figure 6 shows an example device for pre-mapping a space, according to an example embodiment.. More ^■ specifically, me device includes a sensor -602 -configured -to collect sensor data representati ve of an en vironment in which ^' one or more robo ts may later be deployed. In some examples, the sensor 602 may be a two-dimensional nav ation laser sensor capable of producing distance measurements to surfaces in the environment, as well as locations of detected markers. In particular, the sensor 602 may project laser light beams 604 and measure reflected beams to measure distance. The sensor 602 may be mounted at a same height as a sensor o a robot to- be deployed ^' within a space in order to replicate detection capability .of the robot. For instance, the- sensor 602 may be positioned at a height to. detect reirorefieciive reflectors arranged in. a horizontal plane within, an nv ro ent other examples, the sensor 602 may be a two-dimensional laser scanner that only produces distance or contour measurements _* In ^' further examples, the sensor 602 may he another type of sensor, such as a stereo camera ..

|0085f The device additionally includes a computing unit 606 that processes the sensor data from sensor 602. in particular, computing unit 606 ma be configured to rtm any of -the types of mapping functionality described herein ^' to generate maps of the space aid/or use generated maps. The device additionall includes a battery pack 608 for powering both the sensor 602 and the -computing unit 606. The device further includes a. mobile base 6.1 that allows the device to be easily moved through a space in advance- of deploying one or more robots. For instance, the mobile base 610 may be a tripod on wheels as shown in Figu e 6, Other types of mobile bases ma be nsed, including power and/or t powered mobile bases.

|0086| Within examples, the application of .automated guided vehicles (AGVs) and optimizatio of warehouses (with or without AGVs) may require accurate geometric information about the .environment: (e.g.* accurate maps}.. Accurate facility maps may lead to identification of iaciHty/workflow inefficiencies n bot manual and automatic vehicle environments (e.g., racks too close together to allow two power industrial ^' trucks to pass). Facility layouts (e.g., CAD mo ls) are used to guide the building design and .construction process, and may not always updated to reflect the real world. Therefore, they ften contain inaccuracies and. may not be considered trustworthy (e.g., -a GAD may contain correct and accurate walls while racks and other infrastructure added later ma be off by considerable amounts. e.g _t , 30 centimeters), in some examples, a pre-mapping device such as shown in Figure 6 may be used to help generate accurate facilit maps that enable ptimiz AGV and/or manual fork track deployments.

{ΘΘ81] A pre-mapping system may be used for a number of different applications, in. some examples, a map of a warehouse (e.g., a- geometric map confining walls and. other infmstruciure s ch as racks) may be generated and compared, to n existing CAD -model of the space. This process m y reveal that the CAD model does not properly reflect, the teal world. Facility maps may then be updated to the correct layouts _* la further exampl s _* warehouse mapping ma he used to deploy a robotic system in simulation, to demonstrate how the warehouse can be automated using robotic AGVs aad or optimized, using manually- driven powered industrial tracks.

ftOSSj hi additional examples, a warehouse space may be pre-inappe to assist in planning a marker (reflector) setup- te.g., locations to place reflectors in the environment) based o the map. Such a process may he used to speed up the planning phase of an AGV deployment

|ί>089| In further examples, a warehouse map and reflecto setup may be -determined early iu the AGV roilout process before sending any vehicles to the location (or while the vehicles are shipped to save time). Once vehicles arrive, they ma upload the map(s) and stait driving or use the map(s) to initial ize and bootstrap snbsequent mapping runs with vehicles.

fm¾ ^j (ij is some examples, accurate mapping can also he used to determine if a new- generation of manual, trucks will be able to operate in an existing facility or if the facility will require modification, in additional examples, accurate maps may also allow for identification of manual facility inefficiencies like placement, of racking, charging stations, or parkin stations -relative to workflows and travel distances. This information may be used to provide suggestions for more optimal facility layouts.

| _. ft09lj As mentioned, the computing unit 606 may be configured to run various snapping functionality based on sensor data collected by sensor 602, iu some examples _* the mapping functionality may include a hierarchical pose graph SLA and bundle adjustment to build both a warehouse map and a refleciot map. The functionality may produce both maps by switching between contours/di tance measurements and marker detections in order to map a warehouse with only a partial reflector setup or no reflecto -setup at all. The mapping functionality may additionally include automatically aligning generated maps with the CAD model of the warehouse. The mapping functionality ma additionally include bootstrapping the mapping process with a previously built map (e.g., a new map may be automatically aligned with both a CAD model and a previous am , with reflector positions bootstrapped from the previous map),

{0092] An example-process ma includ -retrieving results of a mapping run using the device illustrated in Figure 6, inchkling storage, v¾uaikatio.n., etc. The retrieved results may tlberi be applied for various iimetiofts, including uploading to vehicles for navigation, uploading to vehicles for new mappisig runs (bootstrapped with resulis from the prior run), importing the results into 8 component that shows deployment of a system in simulation, importing the resulis into a component thai compares a generated map with an existing CAD model, and/or importing- the results into a component that assisis with auiomated reflecto planning.

IV, Example Loe f¾aiion of a Robot in an En onmerrf:

| ffi93| Figure 7 shows a rob t navigating wit si an environment,, accor i to an example embodiment. A location and orientation, of robot 700 relative to the eiivironmeni may be estimated, enabling the robo t navigate through the environment accordingly.

Particularly,, the pose may indicate the location and orientation of the robot within the environment

|O094j A computing system may determine the pose of robot 700 based on received signals 706 from one o more sensors 702. Signals 706 provided by sensor 702 may be reflected by rettoretleetive markers placed in various locations in a warehouse. For instance, the robot may use a light ranging and detection (LfDAR.) unit that emits light to an area sniTOttiidiiig the robot, and markers positioned in the area snrroitnding the robot may reflect the light back for detection b a sensor of the robot,

fO095| Reflected signals 706 received at sensor 702 may indicate locations of the markers relative to the robot. A computing system may use these determined locations of the markers to develop a map of the markers, hi some cases, the computing system, may use detected markers to supplement an already generated map of the marker positions. For instance, the computing system may modify the position of one or more marker in the map using new measurements.

f0096| A computing system ma also determine the pose of the .robot as the robot navigates using the map of markers. The computing syste may match detected markers with markers in the map t determine the robot's position and orientation. The locations of the markers in relation to obstacles 710 within, the environment may be predetermined. For example, the locations of obstacles may also be mapped. The robot may make movements 708 to navigate within the environment while avoiding the obstacles based on the estimated pose and the predetermined locations of die obstacles.

10097) Figure- S. sho s a robot associatin detections with mapped landmarks, according to an example embodiment. Detected signals 806 from candidate landmarks 81 may he received by one or more sensors 802 of a roboi 809. The signals may be indicative of locations- of the candidate landmarks in relation to the robot The candidate landmarks may be transformed to align wit mapped landmarks 804, Associations 814 may be formed between live candidate landmarks- and. the mapped landmarks that result in a ^■ minimal distance between the - transformed candidate landmarks and the mapped landmarks. For example, the associations .814 may be formed using a least: means squared ^' method such as an iterative closest point (iCP) method. The candidate landmarks ma be translated and rotated based on the associations 8.14 between the candidate landmarks and the mapped landmarks. The pose of the robot may be mierred by .similarly translating aad rotating the robot

1 98) Figure 9 is a functional block diagram, i ^' Hustiating rriodn!es of a robot control system, according to an example embodiment. The robot control system may include one or more sensors as part of an on-board sensing module 900. The sensors ma provide data that is indicative of ^' wheel odojnetry 908 of the robot, The sensors may also include a navigation scanner 91.0. The navigation scanner ^' 910 may be configured to receiv signals from candida te landmarks in an environment of the robot.

099j A pose estimation module 902 of the robot control system may indicate the location and orientation of the robot with respect io mapped landmark in the environment The pose estimatio .modnle 902 may include software that peribrms itmctions based o» inputs from the on-board sensin modnle: 900, For example, each time the navigation scanner 10 performs a scan, sensor data from the on-board sensin -module may be processed by the pose estimation module 902 to determine a current location and orientation of the robot in the environment. The pose tracking/refinement block 912 and global, localization block 914 of the pose estimation module 902 represent processing steps, while the pose block 916» confidence/accurac block 1K, and initial pose estimate block 920 represent outputs of the processing blocks 912 and 1.4,

100106) The pose estimation module 902 may operate in two modes, in a first mode, the pose -estimation, module 902 may have ah initial pose estimate 920 of the robot and the pose tracking/estiniate block 912 may update the initial pose estimate 920. The pose traeking refineroerit: 912 may utilize the -wheel odo oetry 908 and data from, the navigation scanner 910 In conjunction with, the initial pose estimate 920 to identify the location of the robot in relation to candidate landmarks. The pose tracking/refinement block 912 may associate the candidate landmarks to particular mapped landmarks that are near to the initial pose estimate 920. The pose estimation module 902 may further provide a pose estimate 916 based on the association, and a confidence/accuracy 918 of the pose estimate. The

902 may utilize global localization 914 rather man pose tracking/refinement 912 to determine the pose of the robot. The global localization, block .1 . may test associations etween the candidate landmarks and. mapped, landmarks across the entire environment of the robot. The global localisation block 914 may also output a pose estimate 16 and coafidenceiaceuracy 918. Also in the second mode, the pose 16 and confidence accuracy 918 determined, by the global localization block 914 may be used ^" is the postprocessing module 904 to determine a refined pose estimate of the robot. Further, the pose estimate 916 derived during global localisation 914 may be treated as the initial pose estimate 9.20 of the robot for use in subsequent pose estimations.

{00102] A post-processing module 904 may be used, to -refine the pose estimation derived from, the pose traeking reiMement or global localization. The .post-processing module ma perform fusion extrapolation 922 of the pose estimate and con ence/aecuracy of the .localization module, and wheel odometry of the on-boar sensing module. During fusion extrapolation, the refined pose estimate may rely more on the estimated pose provided by the localisation module when there is a high confidence/accuracy. Conversely, the refined pose estimate may rely more on the wheel odometry when there Is a low confidence/accuracy. Further, the post-processing .module may provide a map update 924 based on the provided confidence/accuracy and refined pose estimate. For example, the map update may update locations of the mapped landmarks based, on the refined pose estimate, in other examples, the map update may update statistical information, associated with, the mapped landmarks used, to generate the refined pose estimation.

160103} Each of the factions performed by the robot control system of Figure 9 may be performed periodically. For example,. ttavi atto» scatmei 910 may perform scans at 8 H¾, while the wheel odometry 908 may update -at 100 Hz. As another example, tire processing blocks 912 and 914 of the pose estimation module .may .receive data from the on-board sensing module at 8 Hz, and may produce poses 916 and confidence/accuracies 1:8 at 8 Hz. Different frequencies are possible as well

V» Exa ple SLAM Itupkymeatations

100104] A computing system may perform SLAM or similar processes to determine a location of a robot while. also detecting positions of markers within fee environment For example, the computing system may detect markers based en the intensity of measurements within laser scans and may match observed markers from different sets of measurements.. fOOluSj Potential errors can arise when attempting to associate detected markers across different: sc ns^ such as -failures that result from a lack -of enough detected markers or wrong, associations betwee detected markers. A computing system may overcome these and other potential errors using a nearest neighbor approach, thai may associate each detected marker to the closest marker that has been detected multiple times. Particularly, the nearest neighbor approach may involve determining the nearest marker using a. Euclidean distance or other distance determinations (e.g., Mahalanobis distance), in. some instances, the computing system may also be configured to utilize a validation sale to determine when a marker has previously been observed or when the marker should be added as a newly detected landmark. The results of the data association between detected markers can enable the computing syste to esti mate a current -pose of the r obot within the environment

{00306| The computing system may use control instructions from the robot's control system when estimating its current pose. For instance, odometry data provided from sensors positioned on wheels of the robot may be used to estimate changes in the current pose of the robot. As an example, a obot. may have an initial 2D position fx,y) with, initial, orientatio Θ. After changin position: by (Ax, Ay} and changing orientation by (ΔΘ) as indicated by the ^• controls- applied ^' y th control system, the updated, pose of the robot is. (x + Ax, y 4· Ay) with updated orientation (θ + ΛΘ), The computing system, may further refine the estimated pose using the detected markers within incoming sensor scans.

fOOlOTj The computing system may also estimate a current pose of a sensor (or the robot) using detected markers and othe measurements of the environment ( .g.,. contour measurements representing distances to nearby surfaces). When the prior pos of the sensor/rohoi is known, the competing system may use the recent pose estimation to predict wher markers should be located based on prior .detection of markers. The .computing system may continue to monitor changes in the robot's pose using subsequent measurements.

{00108] !n some instances, fee computing system may modify the uncertainty associated with each observed -marker to reflect recent changes based on new measurements,: and may also add newly detected markers to the map of markers in the e vi ontnent The -confuting system may perform SLAM iteratively to continuously update the map of markers in (he space '-enabling (he robot to map the environment while also navigating safely,

{001O9) In another example implementation, a computing system may perform a modified SLAM process that uses one or more operations, such as initial pose monitoring, local window optimization, global optimization, and complete bundle adjustment The computing system can often refrain from subsequent operations after die initial pose estimation, In effect,, the computing system, may determine whether or not to perform the next, computationally more expensive operation during perfonftan.ee of the preceding

problems between two nodes simultaneously. 00113] In some cases, inalehmg of laser scans and marker detections within the scans may result in slight errors and inaccuracies. As such, local window optimization may overcome unwarned effects by refining the estimated pose via a pose graph optuBkation in a local window. Particularly, the computing system may optimize the estimated pose while keeping prior poses- of neighboring nodes constant in the pose graph and limiting the use of older pose estimates, that are outside the local window. For example, when scans only produce a few detected markers nearby the robot, the computing system may utilize local window optimization to improve the pose estimate.

f 0114] The com utin system may add a new node to a pose graph when the robot travels a distance or turns ah angle between scans of the env ronment that exceed a. maximum distance or a maximum mm angle. Particularly, th computing system ma add the current scan to the underlying pose graph as the new reference scan that can be used fo su sequent pose determinations,

fOOI iSj The computing system may further ^' determine a local neighborhood of othe nodes in the pose graph and optimize the resulting sub-graph (i.e., the local neighborhood of nodes) to refine the estimate of the robot's current pose as well the other poses represented by the nodes in the local window. The computing system may keep nodes that connect the subgraph to the rest of the pose graph constant and optimize all other poses within the sub-graph to refine the current pose while factoring new scans (Le., information that has not been available when initial computing- the respective poses).

fO0J J6j The computing system may also -perform loop detection and verification. For instance, if the local window contains graph nodes created during a previous pass through that area of the environment, the -computing .system may tri ger a global optimization of the pose graph using the prior scan of the area. Global optimization may optimize all nodes in pose graph representing changes in the robot' s pose and enable building map of marker positions. The computing system may add ah. edge with both, poses to the graph when, a marker is detected from two poses/nodes. By changing the two poses, the computing system may minimize the distances between the two points and obtain new robot poses. The computing system may go through all nodes and extract all marker detections in th world frame to obtain a map,.

f 00117] Durin global optimization, the computing system may match -marker detections (or match contour scans of the- ^' envkonment) thereby form blocks within a large o timisatio problem, that requires all poses in, he graph to be ptimiz d in order to mi imi e the differences between all found matches of markers detected within different scans of the

not optimized as the first pose (circle 1 ) defines the origin of the map coordinate, frame.

1 2 3 4 5 Complete Bundle Adjustment:

All poses (circles 2 _S 3,4, 5) and mapped markers (donuts 1, 2, 3, 4, 5} are globally optimized. Only the first pose (circle 1} is not optimized-

TABLE 2

{ 001203 The computing s stem may continue to update the map of marker positions by aggregating marke detections of aligned scans. For each detected marker, die computing system ma check to see if the detected marker matches a marker already added to the map, which may involve using a nearest neighbor search with a maximum distance corresponding, to the expected inter-niarker distance (e.g., 0.5 meter distance threshold) to identify matches between markers detected, within multiple scans of the environment. If the computing system finds that a detected marker matches a marker positioned in the map, the computing system may adjust the position of the marker within the ^' map if needed, in some iastattces- the computing system may speed up the matching process using an ap|¾xx iniate nearest neighbor search in M-tree of th map of markers tha can be rebuilt after adding new markers or rebuilding the map in general,

00121 j In some examples, a computing system may determine matches by going through ail. points i a set. and matching each point with the closest point, in another set of points. In particular,, closest points within distance thresholds may be considered matches, Irs other examples, a computing system may use kd-trees or other search structures that ma enable searching for matches faster.

[001223 fa order to identify marker detections that are likely false detections, the computing system may monitor a local detection history to determine whether the marker is stable.. Stable markers are included in the final map of marke positions as either new markers or to correct previously added markers. The computing system may distinguish an intermediate map representation mat contains information about all markers detected within, scans of the environment from a final map of the marker positions that only contains stable markers and «o false detections likely caused by highl reflective surfaces.

fO0123| Iti an example ^mbodimeai, die com ilin system may mooitof a local detection history for each mapped marker to remov false detections. For instance, in a local window of w scans, die computing system may add a. negative (.for -every mapped marker) for every scan added and a positive if the scan contained a marker detection matching die mapped marker. As example of the local window of detection is shows below in table 3,

detections equally, but may -use incremental weighted .mean i» sortie- examples to give- markers detected closer to die sensor a higher weight than markers detected far away f in the robot since the noise m. the- measured positions likely increase with distance. The computing system may also compute an incremental eovarianee for the aggregated .position to. provide an additional, way of discriminating false from, true marker detections- since a false .detection may likely show a larger variance. Further, fee computing system may also switch from point- to-point errors to covariance-based error functions in pose monitoring and the cost functions of the graph, optimizations.

VI. Example Systems ami Methods

{00128] Figure 10 is a .flo chart illustrating a method 100 , according to an example implementation. Particularly, method 1000 may be implemented to perform localization and calibration,:

|00129 j .Method 1 00 (and other processes and methods, disclosed .herein) presents a. method that ca be implemented within an arrangement involving, for example, robotic device(s) 1 0 (or more particularly by components or subsystems thereof, such as by a processor and a non-transitory computer-readable medium, havin insiructions that are executable; to cause the device to perform functions- described, herein). Additionally or alternatively, -method 1000 may be implemented within any other arrangements and systems. 160130} _. Method 1 00 and other processes and methods disclosed herein may include operations, func tions, or actions as .illustrated by one or more of blocks 1002, 1004, 1006, and 1008. Although the blocks are illustrated in sequential order, these blocks may also b performed in parallel,, and/or in a different order than those described herein. Also, the various blocks may be combined into fewer blocks, divided; into additional blocks, -and/of removed based upon the desired, unplemenf ation.

i( _. 0013 ^' lj In addition, for the method 1000 and other processes and methods disclosed herein, the flowchart shows fimciionaiity and operation of one possible implementation of present implementations, in this regard, each block may represent a module, a segment, or a portion of program code, which includes one or more instructions executable by a processor for implementing specific logical functions or steps in the process. The program code may be stored on. ny type of computer readable medium., for example, such as a storage device including a disk or hard drive. The computer readable medium may include non-transitory computer readable medium, for example* such as computer-readable media thai stores data for short periods of time like register iBemoty, processo cache and Random Access Memory (RAM), P0i32| The computer readable medium may lso include Ron-frsaisitor media, such as secondary or persistent long term storage, like read only memory (ROM), optical or magnetic disks, conipaet-dise read only memory (CD-ROM), for example. The comparer readable media may also be my other volatile or oou- volatile storage systems and may be considered a computet readable storage medium, for example, or a tangible storage device. In addition, for the method 1000 and other processes aid niethod disclosed herein,, each block in Figure i t ⁾ may represent circuitry that is wired to perform the specific logical Junctions in the process.

{ ^' 00133] At block 10(12, method 1.000.may include receiving sensor data indicative of a plurality of markers detected by a sensor on a vehicle located at a plurality of vehicle poses within an environment. A computing system (e.g., control system, remotely positioned computing system) ma receive a series of sensor measurements of the environment from a sensor or sensors coupled to a vehicle (robot) while the vehicle navigates the environment As a result, the computing system ma receive sensor data that represents positions of markers in the environment from, different poses of the vehicle as the vehicle continues to change position and orientation relati ve to the e iron ent,.

{0013.4] 1B some examples, the computing system may receive scans from a laser scanner as the robot navigates. The computing system may detect landmarks (e.g., markers) positioned in the environment within the incoming laser scans. For instance, the computing system may receive measurements that include detect markers arranged along horizontal plane from a 2D laser scanner on the robot. Accordingly, the computing system may detect, markers, in the horizontal plane in each scan from the laser scanner. In other examples, the computin system may receive sensor data from. -other types of sensors coupled to a mobile robot, such as camera images, .RADAR _* etc.

{00135] At block 1004, method 1000 may include -determining pose graph representing the plurality of vehicle poses and the plurality of markers. In particular, the pose graph may include a plurality of edges associated with a cost function representing distance measurements between matching marker detections at different vehicle poses. For instance, the pose graph may include edges that result from pairwise .matching of markets detected from sensor data provided at different- vehicle poses. The distance measurement ma incorporate the different vehicle poses and a sensor pose on the vehicle.

00136) As a robot navigates an environment, a computing system may perform SLAM; Or a similar process to determine different poses of the robot relative to the environment lor the pose graph. Each robot pose may represent a position and orientation of the robot with respect to markers positioned at various locations within the e¾tVirott«tent. Although tiie measurements of the environment ^' are provided by a sensor positioned on the robot, the computing system may use a fixed sensor to robot transform that assumes that the sensor remains constant relative to the rob t _* In such a configuration _* the co futin system may use incoming scans to deteraune. new robot poses and update the pose graph to include ^' the additional robot poses. The computing -system ma also simultaneously develop a map of the markers thai specifies locations of the detected makers within di environment. Similar to updating the pose graph, the computing system may also refine the map of markers as new sensor measuremen s are received from the sensor of the robot.

I37| SLAM or similar methods for localising a robot within the environment may; result in potential inaccuracies- To account for live inaccuracies, the computing system may perforin iterations of SLAM or other processes to modify, and update robot poses and marker positions based on newly acquired measurements _* In some cases, the ew measurements may confirm the prior poses and positions of ^' the makers, in others, however, ihe computing system may update one or more prior robot poses and/or prior positions of markers in the map.

00i38j As indicated above, a computing system may hold the sensor pose relative to the robot constant when determining robot poses within the environment in some instances, however, the position or orientation of the sensor may change .relative to the robot, which rnay result in errors when determining robot poses or positions of markers within the environment. The sensor pose may cau e other inaccuracies within examples,

{00139! To account tor potential inaccuracies that might arise from the pose of the sensor while performing SLAM, a computing, system, may develop the pose graph to involve a cost function that incorporates the pose of the sensor. While performing localization (or subsequent to localization), tire computing system .may then identify. hen the sensor pose differs f om prior expectations,

{001.40] At block 1006, method 1000 may include determining a sensor pose ^' transform representing the sensor pose on the vehicle that optimizes the cost function associated with the plurality of edges in the pose graph, in particular,, the determined sensor pose transform may indicate a position and an orientation, of the sensor relative to a coordinate frame fixed to the vehicle. As such _? the determined sensor pose transform may be used to avoid errors that might arise when using a fixed sensor pose transform.

f 00141) I some examples, the computing system may determine a sensor pose transform white performing SLAM. For instance, the computing system may determine robot poses d positions of arkets within die environment usin mcoisittg sensor data and then proceed to determine the sensor pose -tt&nsfomi iliat oplirtiizes die cost i unction. Th computing system may lternat between updating the robot poses and positions of markers with updating the determined sensor pose transform. The computing system ma use matches, of markers detected at different robot poses to assist in updating: the .sensor pose. |ίΚ}Ι42| In further examples, the comparin system may simultaneously, determine the sensor pose ^■ transform arid robot poses relative to a map of markers in order to optimize the cost function associated with the edges in the pose graph. For instance, the computing system may determine th sensor pos teUstorm as the robot navigates within the environment incorporating both new measurements and new robot poses when determining the sensor pose transform. The new measurements and robot poses may also be used to adjust the sensor pose transform. Accordingly, the sensor pose transform may be contiguously or periodically updated using new information to minimise potential errors, |0OI43j Figure 11.A illustrates -a pose graph, m accordance with example embodiments. As shown, pose graph 1 100 includes robot poses 1 IQ2, 1 ft. 1 110, sensor poses 1 104, 1 108, 1 1 12, and marker detections 1 1 14, 1 116, 11. I S, and. 1 120.

|0O144| Pose graph 1 100 represents sensor calibration as a variant of a pose graph- based SLAM problem in which tire sensor pose of the robot is a parameter being optimized. An example cost iunetion based on pose graph 1100 represents the distance Between matching detected markers- in scans of the en vironment incorporating both robot poses in. the map of markers and sensor pose an the robot, Table 4 represents example cost fu ctions- associated with pose graph 11.00.

|00I45] Cost function 1 and cost function 2 -Shown in Table 4 represent two possible cost functions' that may be optimized in order to optimize sensor pose calibration as part of a graph-based SLAM process. When using cost function I, a computing system may use pairwise detection -whenever a marker is detected- -at multiple robot poses. For e ample, .marker detection 1114 may be detected at both robot pose l itis and robot pose 1 106, A computing system may insert the values (marker detection 1 114 at both robot poses i 102, robot pose 1 106) into cost function I to determine- a first edge and cost func ion o mimmize to reduc error. The computing system may repeat this process for other detected markers and robot poses.

{ ^' 00146] Cost function 2, however, may require an accurate map that enables matches against a .mapped, marker location instead of pairwise matches between scans. A computing ^• system may use cost ^' nction 2 with a separate edge for each of robot poses 1102, 1106 as suming the map indicates an accurate position o f the marker 1 1 14.

|001 7] Robot poses 1102, i 106, and 1110 represent sets of positions and orientations of a robot moving within an. emironrnent and may be determined using SLAM, or similar process that involves marker detections 1 11 1 1 16, 1 1 -18, and 1 120, A computing system may receive sensor measurements from a sensor on the robot and determine the sequence of robot poses I Ϊ02, 1106, and 1 1 10 relative to markers detections 1 1 14, .1 1 16, 11 18, and 1120 as the robot navigates within the environment. For instance, the computing system may determine robot pose 1102 using marker detection 1 1 14, 1 16, 1 18, and further update robot pose 1 102 based on subsequent measurements of the environment. The computing system may also determine a rnap of markers during SLAM based on marker detections 1 1 14, 1 1 16, 1 1 18, and 1 120.

{ ^' 00148] As indicated above, when tire computing system detects- the same marker (e.g., marke detection 1 114) trots two different robot poses (e.g.., robot poses .1 .102, 11.06), the computing system, may ad another edge to pose graph 1 100. The edges, however, might include inaccuracies thai may be accounted, for using additional iterations of SLAM to minimize ail edges. The computing system may adjust simultaneously adjust robot poses 1102, 1 106, and 1.1 10 and positions of markers detections 1114, 1 1 16, 11.18, and 1 120 t minimize all edges .

{001.49} in some examples, the computing system may also determine and adjust sensor pose transform- to reduce potential errors that may arise from the pose of the sensor relative to the robot Particularly, the computing system may adjust the sensor pose transform when the computing system identifies a different sensor pose than expected. As such, the computing system may identify differences among sensor poses 1 104, 1108, and 1112 that may arise as the robot navigates the space and the computing system performs SLAM or a similar process. fOOlSOj At block 10Θ8, method 1000 may include providing the sensor pose transform representing th sensor pose on the vehicle. For instance, the computing system may control the robot to navigate withi th environment based n the determined sensor pose transform. The competing system may also store the sensor pose transform for subsequent use.

100.151) In some -examples, a computing system may determine a subsequent pose graph representing subsequent robot poses and detected markers. The computing, system may ^■ utilize additional sensor data received from the sensor when me robot is at the subsequent, robot poses. Using iiie . ^' subsequent pose graph, the computing system may determine another sensor pose transform that can fee used to cheek the original sensor pose transform. In some Instances, ^' the _. a ditional sensor pose tosfbr may va idate the original sensor pose transform, In other instances, however, the eompatmg system may identify a difference ^' between the origiriai sensor pose transform and the additional, sensor pose transform.. If the sensor poses differ, the computing system ma provide a -calibration error signal to an. operator or another receiver (e.g., robot control system) that indicates the difference;

|00i 5 1 la further examples, a computing system may determine .robot poses relative to a map of the markers to optimize a cost fonction associated with edges in the pose graph. As discussed above, the map of markers may be generated using detected markers as the robot navigates within the environment. As such, the computing system may determine a sensor pose transform and robot poses relative to the map of markers simultaneously »j order to optimize the cost iroe-tion. The simultaneous determination may improve the efficiency associated with deten ning the sensor pose U'anslorm and robot poses,

|00! 53| i other examples, computing system may determine a sensor pose transform subsequent to determining robo poses, for instance, the computin system may determine robot pose within the environment (with or without the map of markers) and proceed to determine a sensor pose transform after the robot poses. The computing system may hold robot poses fixed while determining iiie sensor pose transform subsequent to the robot poses, {00!S4| ϊή some examples, the computing system may determine the pose graph such thai the pose graph includes additional edges associated, with, aft additional, cost function. The additional cost function ma represent one or more additional distance measurements between marker detections at robot poses and mapped, marker positions. The ad itional distance measurements may incorporate robot poses and the sensor pose on the robot Accordingly _. , tlie computing system may determine a sensor pose transform such thai the sensor pose transform also optimizes the additional cost function. In some instances, the computing system may also cause the robot to make series of movements based oft previously mapped marker positions to determine the sensor pose-tiaiisfomi {00155| In other examples, sensor pose transforms for other sensors on ilie robot may also be determined. . $ an example, a computing system may receiv additional sensor data that indicates positions of markers fiw a second sensor on th robot when the robot is located at robot poses within an environment The computing system may determine a pose graph that further, includes, additional edges based on the sensor data from the second sensor and the second sensor pose on the robot. The pos graph incorporating additional edges may be -used to determine a second sensor pose transform that represents the second sensor pose on the robot. Th computing system ma also provide the seeoad sensor pose transform to the robot cont ol system or other computing devices..

β0156] in some examples, a computing system may perform sensor calibration and motion model calibration together, hi particular, the computing system, ma determine a pose graph that includes edges for both sensor calibration, and motion model calibration. The computing system may then use localization, results to build an aligned pose graph and use the aligned pose graph to optimize the ^■ motion model parameters. The computing system, may subsequently optimize senso calibration.- Via- marker -detection edges. In some instances, the computing system may optimize the pose graph to fine-tune localization results and responsiveJ re-compute sensor calibraiion and motion model parameters until reaching convergence.

{00157] In -further examples, a computing system may dete mine the pose graph to further include additional edges associated with an additional cost -function representing an error in consecutives robot poses based on a robot motion model The computing system .may determine one or more motion model parameters for the robot that optimize the additional cost function associated, with the additional edges in the pose graph. The one or more motion model parameters may include one or more of a turning delay, a wheel diameter, a turning radius, and a center point of rotation. For instance, the computing system, may determine the one or more motion model parameters while the edges in the pose graph and the sensor pose transform are held fixed. The computing system, ma also provide the one or more motion model parameters for the robot. In some cases, the computing system .may also ^' update one or more of the various robot poses in the pose graph based on. the one o more motion model parameters ^' . Accordingly, the computing system ma subsequently adjust the sensor pose transferal.

100158] Ih some cases, errors May arise when incorporating odometry estimates when localizing a robot. In pariiealar, a computing system may use motion model to transform measured steering and tmetkai velocities into odonietrie motion and pose estimates. When a .motion tnodei. is accurate, the computing ^' system may associate a higher confidence level with the odometiy - estimates: when performing localization. When a motion model is less accurate, h wever, the computing .system may retrain irom depending on the- odometry estimates for localization. For instance, the competing- system .may avoid using dometry'' estimates whe the robot is turning on the spot since the sensor scans may be distorted.

{00159] Figure 11 B illustrates a pose grap thai incorporates motion model parameters, in accordance with example embodiments. Similar to Figure .1 1. A, pose graph H00, as shown in Figure I lB, further includes motion edges 1122, Π24 representing motions performed- by the robot device between ^' respective robot poses 1 1 2, 1 106, and 1 HO. Pose graph 1100 wit motion edges 1 ! 22, 1 124 may be used ^' to optimize one or more motion rsiodel parameters of t he robot.

|00!.6i ^' S A computing system ma develop pose graph. 1.1.00 to incorporate motion edges 1122, 1 124 based on. expected motions of the robot between robot poses. The computing ystem: may further optimize motion, model parameters and then optimize sensor calibration looking at marker detectio edges. The computing system may repeat the -process until reaching convergence,

{00.161 j In some examples, a computing system may further incorporate motion model parameters of the robot that can be optimized in addition to or instead of sensor -calibration. A computing system may use edges in pose graph .1 100 that relate only to robot poses 1102, 1 1 6, and 1 1 1 and not rely upon detected marker detections ! 114, 1 1 1:6, 1 1 18, and 1 1.20 whe determining motion model parameters. Particularly, the computin system may compare consecutive robot poses (e.g., robot poses 1 102, 1 106) to predicted motions of the robot ie.g., motion edges 1 .1 2, 1 124) based on motion model parameters,

{00162] if the motion model parameters are inaccurate, the estimated robot morion may systematically deviate from, actual robot motion. For Instance, the computing system, may analyze wheel odometry, time betwee scans, control instructions, aud or other .factors when determining the robot motions represented by motion edges 1122, 1 124 in pose graph HKl Particularly, the .computing system may determine pose graph Π0Ο to include additional edges (e.g,, motion edges ί 122, 1 124) associated with an additional cost function representing --an erro in consecutive robot poses based on a robot motion model.

{001.63] The computing system: ma then determine one or more motion model parameters for ^' die robot that optimize the additional cost function associated with motio edges 1 122, 1 124 in pose graph 1 100. For example, motion model parameters may include -a turning delay, a wheel diameter, a turning radius, and a center oint of rotation. la some instances, the c nfuting system .may hold the edges in pose graph 1100 and the sensor pose transform fixed when - determining the one or more motion model parameters. The motion model paramete s) for the robot may be- provided to the robot control system or other computing systems.

|00164 1B some examples, sensor pose calibration and .motion mode! calibration may be performed- separately or together (e.g., alternating sensor pose calibration and motion .model calibration). In forther examples, the computing system may update one or more of robot poses i 102, 1 106* 11 10 to. pose graph 1 100 based n the one or more motion model parameters. The confuting system may also subse uently adjust the sensor pose transform after updating one or more robot poses 1 102,.1106, and 1 I 1.0 in pose graph 1 100.

{00:1651 In a further example, a computing system ma b ild a graph of poses using different types of edges (and different cost functions fo the underlying optimization problem). The pose graph may include pairwise edges detected between matching marker detections in. two or .more scan and edges between marker detections and mapped marker locations. By incorporating the sensor pose on the robot, me computing system may optimize the sensor calibration. For optimizing motion model parameters, the computing system may use edges encoding ^' relative robot motions (e,g., odonietry), such as a 3-D cost for the motion and the difference in heading. The computing system may initialise the motion model parameters with the ^' localization results to determine an updated motion model. In. some instances, tire computing system may alterBate between sensor calibration and motion model calibration.

|00l66j in some examples, a computing system ma simultaneously .localize the robot and refine- parameters of a -determined ^' motion model. For instance, the computing, system may estimate tire most likely robot trajectory and use the trajectory to optimize and check a sensor pose on the robot. The computing system may also optimize and check on the motion parameters. As a result, the computing system may identify discrepancies between, expected parameters and actual parameters to provide a signal when the robot ma require recalibraiion. For instance, the computing, system may provide an alert to an, operator.

1001671 In a further example, a computing system; may perform an initial vehicle calibration by causing a- vehicle to move .forward and backwards- while- -also turning left and right. By localizing the vehicle within the environment, the computing system may obtain pose estimates and motio estimates and use the estimates together with the original sensor data in a SLAM: process to ^' compute an updated sensor calibration and/or updated, motion. mode! parameters.

[0016$! In another example, a computing system, may perform an automatic calibration vahdation process, which may involve collecting logged data from vehicles over a period of time (e.g., one day). The computing system may then feed, ^' both the localization results and the logged sensor data into the same graph-based SLAM, system and determine ^' updated sensor calibrations and/or motion model parameters, if the updates show the same differences over several iterations, the computing system may confirm that prior calibration aiid/pr motion model parameters used previously are off re ninog subsequent calibration, for instance, the computing system may provide a signal to an. operator reporting updated values thai may replace prior ^' calibration values,

VII, Other Localization Emb& iiueais

|0O169| Tfcough the detailed description, above generally relates to simultaneous locaiizatioii and calibration ^' involving robotic vehicles within an environment it should be understood ^' that such des ription is provided for purposes of example, and should not be construed as limiting. Fo instance, the same systems and methods described above may be implemented to localize a man a liy Operated vehicle i the environment. Such localization may allow a human operator to view a map of the environment provided for display on the manuaJly-oper ated vehicle that includes an indication of a current pose estimate of the maBuaiiy-opersied vehicle.

lOOITOj Further, the systems and methods described above may be im lemented, to localize other devices, such as a mapping sensor unit. For example, a mapping sensor unit, may be moved to multiple positions within an environment to generate data used for mapping the environment At each position, the sensor unit may be localized using one or more of the systems and methods described above. Such data ma be used in generating a ma of the environment it should be readily understood by those having skill in the art that the systems and methods describe can be implemented to localize any number of vehicles, devices, or sensors disposed within an .environment

VI if. Conclusion

[00171 j The present disclosure is not to be limited in terms of the particular implementations described in this application,, which are intended as illustration of various aspects. Many modifications and variations can be made without departing from its spirit and scope, as will be -apparent to those- skilled in the art Functionairy equivalent methods and apparatuses within the scope of the disclosure, in addition io those enumerated herein, will be apparent to those skilled in the art from the foregoing descriptions. Such modifications and. vacations- are intended ^' to fall wft a the scope of ti e appended claims,

f 001721 The above detailed description describes various features and functions of the disclosed systems, devices, and methods with reference to the accompanying figures , In the figures, similar symbols typically identify similar components, unless context dictates otherwise. The example implement tions described herein and in the figures- are not meant to be limiting. Other implementations can be utilized, and other changes can be made, without departing from the spirit or scope of me strbjeet matter presented herein, it will be readily understood that the aspects of the present disclosure, as generally described herein, and illustrated m the figares, can be afftmge _* substituted*, comb ned, · separ ted, atid desigded in a wide Variety of dlSerent conligvsrations, all of which, are explicitly contemplated herein. 100173] The particular arrangements shown in- the figares should not be viewed as limiting, it should he understood that other inipSementa lions can include more or less of each element shown in a given figure. Farther, some of the illustrated elements can he combined or omitted,. Yet further, an example implementation can include elements that are not illustrated in the figures.

1-001741 While various aspects and implementations have been disclosed herein, other aspects and implementations will be apparent to those skilled in the art, The various aspects and implementations disclosed herein are for purposes of illustration and are not intended to be limiting, wit the tee scope being indicated by the .following claims.