Patent Description:
The present invention relates to a tracking system for tracking the position and/or orientation of an object in an environment.

The reference in this specification to any prior publication (or information derived from it), or to any matter which is known, is not, and should not be taken as an acknowledgment or admission or any form of suggestion that the prior publication (or information derived from it) or known matter forms part of the common general knowledge in the field of endeavour to which this specification relates.

To accurately control the end effector position of industrial robots or large construction robots, it is necessary to measure the position and orientation of the end effector, or a part of the robot close to the end effector. To achieve dynamic stability and accurate control during movement of the robot, it is necessary to measure the position and orientation at a high data rate and in real time. Delays in the feedback loop of a control system lead to following error and reduced bandwidth and phase margin, all of which are undesirable. Delay can also introduce self-excitation or resonance in servo-controlled systems.

For conventional sized robots with a reach of up to <NUM> and when the end effector maintains line of sight to a laser tracker, the currently available laser tracker and Optical CMM solutions provide accurate data at adequate frequency to correct the end effector position for tasks such as drilling.

For large construction robots such as that described in the inventor's international patent application <CIT>, which has a reach of <NUM>, and where due to line of sight constraints, the distance between the target and the end effector can be <NUM>, the static position measurement accuracy of about <NUM> and orientation accuracy of <NUM> degrees results in an end effector accuracy of only <NUM> which is only just adequate. With an orientation accuracy of <NUM>, the end effector accuracy is reduced to +/-<NUM> which means that adjacent bricks could have a mis-match of <NUM> between them, although the inventor has found that typically the bricks are within <NUM> of each other and within <NUM> of absolute position. Furthermore, the position measurements have error components that consist of long term drift, high frequency white noise and low frequency noise (that may be due to vibration of structural parts of the measurement system or tuning of servo components in the measurement system). Filtering the measurements to reduce the noise introduces a time delay. Even with an optimised system, the introduction of error noise and delay reduces the dynamic performance of the coarse-fine position compensation system and can introduce resonance to the system.

Six degree of freedom laser trackers such as the Leica AT960XR with Tmac or the API Omnitrac with Active Target or Smart Trac Sensor (STS) are known. The Leica AT960XR laser tracking system can provide position coordinates accurate to approximately +/-<NUM> at a rate of <NUM> positions per second. The AT960XR with Tmac probe can also provide orientation to an accuracy of <NUM>. 01degrees and this orientation is measured at <NUM> and either interpolated to <NUM>, which introduces a <NUM> time delay or extrapolated to <NUM> which introduces an extrapolation error which depends on the motion of the Tmac. With the Tmac, the range is limited to <NUM>. The cost of a system was approximately AUD <NUM>,<NUM> in <NUM>.

The API Omnitrac and STS provides both position and orientation data at <NUM>. The orientation data has an accuracy of less than <NUM>. API may, in the future, improve the orientation accuracy of their equipment to <NUM> degree but this has not happened yet.

Laser trackers rely on measuring the time of flight of a light beam and/or laser interferometry for distance measurement and therefore depend on accurately knowing the temperature and density of the air because this affects the speed of light. For this reason the laser trackers include a weather station to measure temperature and humidity and barometric pressure.

GPS (Global Positioning System) with RTK (Real Time Kinematics) is known to provide horizontal position accuracy at approximately cm resolution at rates less than <NUM>. The height accuracy of GPS with RTK is worse than this.

The Nikon iGPS can provide position accuracy of <NUM> and full 6DOF (six degrees of freedom) position measurement, however the measurement rate is limited to approximately <NUM>.

The Nikon K Series optical CMM (Coordinate Measuring Machine) uses three linear CCD cameras to localize infra-red LEDs incorporated into the carbon fiber housing of a touch probe. The measuring range is limited to <NUM> distance and provides a volumetric accuracy of <NUM> to <NUM>. Orientation accuracy depends on the size of the probe. The measurements can be at up to <NUM>.

Laser trackers have moving components and require precise alignment and calibration on a regular basis. They are relatively delicate instruments. They require particular care when being used on construction sites and when being transported. A laser tracker unit must be set up on firm ground when used on a construction site.

The realities of a construction site using large robots require a robust position and orientation tracking device that can measure <NUM> degrees of freedom and provide velocity data as well, over a large volume, and that is easy to set up and transport. An order of magnitude reduction in the price of the system, relative to currently available systems would be highly beneficial. A target price in the tens of thousands of dollars range rather than hundreds of thousands of dollars range would be desirable.

It is against this background, and the problems and difficulties associated therewith, that the present invention has been developed.

In one broad form, the present invention seeks to provide a tracking system for tracking the position and/or orientation of an object in an environment, according to claim <NUM>.

In one embodiment, the plurality of cameras are spaced radially apart with their central axes lying in a common plane.

In one embodiment, the plurality of spaced apart targets include a plurality of target arrays of predetermined configuration, each of the target arrays having at least three targets spaced apart from each other by predetermined distances.

In one embodiment, a target array has a unique spaced apart configuration of targets, compared with any other target array.

In one embodiment, a target array includes at least one of:.

In one embodiment, the targets are lights that are time synchronised to switch on and off at defined intervals to thereby allow a camera imaging a target to identify the target that is imaged.

In one embodiment, the body includes a surveying target viewable by a surveying instrument to determine the position of the body relative to the surveying instrument.

In one embodiment, the camera array is used to determine the orientation of the body.

In one embodiment, the system includes at least one initial reference target at a known location in the environment and wherein the one or more electronic processing devices are configured to:.

In one embodiment, the initial reference target is removed after a predefined number of random targets have been positioned in the environment by the one or more electronic processing devices.

In one embodiment, the system determines new targets as the object moves through the environment.

In one embodiment, a target position in an image is determined by the one or more electronic processing devices analysing pixel target saturation, to determine pixel array coordinates for the centre of the target.

In one embodiment, the targets use colour to distinguish individual targets in a target array.

In one embodiment, triangulation is performed on the processed image data to determine at least the distance between a target and a camera.

In one embodiment, the pixel array coordinates corresponding to the position of a target are determined to sub-pixel resolution.

In one embodiment, the pixel array coordinates corresponding to the position of the target in the image are indicative of angular data representing a target heading angle and an elevation angle.

In one embodiment, previously stored images are analysed by the one or more electronic processing devices to determine a rate of change of the target heading angle and target elevation angle for use in determining the velocity of the object.

In one embodiment, the system further includes a look-up table of calibration data stored in memory of the one or more electronic processing devices, the calibration data including pixel position values and range correlated to camera focussing data, so that observed target pixel array coordinates have camera focussing data applied to thereby apply range correction in the determination of distance to targets.

In one embodiment, the system includes a camera array including two or more cameras mounted around the object in a distributed arrangement.

In one embodiment, the at least one camera is a digital camera having one of:.

In one embodiment, each camera is connected to a dedicated image processor for processing the image data from the camera.

In one embodiment, the image processor of each camera sends data via a data bus to a dedicated position and velocity processor that determines the position, orientation and velocity of the object.

In one embodiment, the position and velocity processor outputs data to a control and logging system via a fieldbus link.

It will be appreciated that the broad forms of the invention and their respective features can be used in conjunction, interchangeably and/or independently, and reference to separate broad forms is not intended to be limiting.

Examples of the present invention will now be described with reference to the accompanying drawings, in which: -.

An example of a tracking system <NUM> for tracking the position and/or orientation of an object <NUM> in an environment E will now be described with reference to <FIG>.

In this example, it is to be understood that the object <NUM> typically forms part of a robot assembly <NUM> and in the example shown in <FIG> the object is a robotic component such as a robot base <NUM> which supports a robot arm <NUM> and end effector <NUM> programmed to perform interactions within the environment. The robot assembly <NUM> is positioned relative to an environment E, which in this example is illustrated as a 2D plane, but in practice could be a 3D volume of any configuration. In use, the end effector <NUM> is used to perform interactions within the environment E, for example to perform bricklaying, object manipulation, or the like.

The term "interaction" is intended to refer to any physical interaction that occurs within, and including with or on, an environment. Example interactions could include placing material or objects within the environment, removing material or objects from the environment, moving material or objects within the environment, modifying, manipulating, or otherwise engaging with material or objects within the environment, modifying, manipulating, or otherwise engaging with the environment, or the like.

The term "environment" is used to refer to any location, region, area or volume within which, or on which, interactions are performed. The type and nature of the environment will vary depending on the preferred implementation and the environment could be a discrete physical environment, and/or could be a logical physical environment, delineated from surroundings solely by virtue of this being a volume within which interactions occur. Nonlimiting examples of environments include building or construction sites, parts of vehicles, such as decks of ships or loading trays of lorries, factories, loading sites, ground work areas, or the like.

A robot arm is a programmable mechanical manipulator. In this specification a robot arm includes multi axis jointed arms, parallel kinematic robots (such as Stewart Platform, Delta robots), spherical geometry robots, Cartesian robots (orthogonal axis robots with linear motion) etc..

An end effector is a device at the end of a robotic arm designed to interact with the environment. An end effector may include a gripper, nozzle, sand blaster, spray gun, wrench, magnet, welding torch, cutting torch, saw, milling cutter, router cutter, hydraulic shears, laser, riveting tool, or the like, and reference to these examples is not intended to be limiting.

It will be appreciated that in order to control the robot assembly <NUM> to accurately position the end effector <NUM> at a desired location in order to perform an interaction within the environment, it is necessary to be able to accurately determine the position and/or orientation of a reference point on the robot assembly.

In this example, the tracking system <NUM> includes at least one camera <NUM> mounted to the object <NUM>. As will become apparent from the following description, preferably the system <NUM> will have more than one camera mounted to the object so as to form a camera array with a wide field of view of the environment in which the object <NUM> is moving.

The tracking system <NUM> further includes a plurality of spaced apart targets <NUM>, <NUM>, <NUM>, at least some of said targets viewable by the at least one camera <NUM>. In this example, a plurality of target arrays <NUM>, <NUM>, <NUM> are shown which in turn each have a plurality of clear and defined (i.e. discernible) optical targets. In this example, the targets are positioned around the environment at known locations and are used as reference markers which allow the system <NUM> to determine the position and/or orientation of the object <NUM>. Whilst targets such as that shown in <FIG> are manually placed around the environment, this is not essential and in some examples, the targets may be fixed points of reference in the environment that are selected by the system as targets as will be described in further detail below.

The system <NUM> further includes one or more electronic processing devices <NUM>, <NUM> configured to determine target position data indicative of the relative spatial position of the targets. The target position data is indicative of the position of the targets in the environment (i.e. in an environment or world coordinate system ECS). This data comprising the relative spatial position of the targets may be manually input to a processing device by a programming interface such as a keypad or may be provided as data measured by surveying equipment such as a total station used to survey the environment (e.g. a building site) once the targets have been placed. Alternatively, the target position data may be determined by creating a cartesian map of the environment with computer selected targets (such as fixed landmarks or reference points in the environment) forming part of the map and their positions determined relative to an environment coordinate system assigned to the map.

The one or more electronic processing devices <NUM>, <NUM> then receive image data indicative of an image from the at least one camera <NUM>, said image including at least some of the targets <NUM>, <NUM>, <NUM>. The image data is then processed to identify one or more targets in the image and determine pixel array coordinates corresponding to a position of the one or more targets in the image. In this regard, it will be appreciated that typically the one or more targets are uniquely identifiable so that the processor is able to discern which target is in the image. This may be achieved in numerous ways as will be described in more detail below.

Finally, the one or more electronic processing devices use the processed image data to determine the position and/or orientation of the object by triangulation. Typically, the position of the one or more targets in the image is resolved to sub-pixel resolution in order to increase the accuracy of the triangulation. It will be appreciated that in some examples, the triangulation is performed using two cameras imaging one or more targets (preferably at least two targets) whilst in other examples the triangulation may be performed based on one camera imaging multiple targets. Any suitable method of performing triangulation known to a skilled person may be used in order to determine distance or range and orientation between the one or more targets being imaged and the at least one camera.

It is to be understood that the above tracking system may be configured so that the triangulation is well conditioned in order to provide accurate location. According to the invention, an array of cameras and targets are provided so that as orientation and position of the object changes, the triangulation remains well conditioned. The concept of the triangulation problem being poorly/well conditioned is illustrated schematically in <FIG>. In <FIG>, a system that is poorly conditioned is shown in which cameras <NUM>, <NUM> are closely spaced together viewing a target <NUM>. It is to be appreciated that alternatively a camera <NUM> could be viewing closely spaced targets <NUM>, <NUM>. In this example, the position of the target is at <NUM>. If the measured angle from the cameras to the target has a small error α this will translate into significant range uncertainty to the target represented by big Δ. The apparent position of the target then becomes <NUM>' and an accurate distance to the target is therefore unable to be determined by triangulation.

Conversely, in a triangulation problem as shown in <FIG>, the cameras <NUM>, <NUM> are sufficiently spaced apart viewing target <NUM> so that a small angular error α between a respective camera and a target will translate into small range uncertainty to the target represented by small Δ. The apparent position of the target then becomes <NUM>' and an accurate distance to the target can still be determined by triangulation. It is to be appreciated that <NUM>, <NUM> may alternatively represent targets with a camera <NUM>. The present tracking system is preferably optimised by selecting the number of cameras and targets and their spacing to ensure that wherever the object is in the environment, the triangulation problem remains well conditioned to ensure that accurate 6DOF position and orientation measurements can be obtained.

The above described tracking system <NUM> provides a number of advantages. Firstly, it provides a camera based system using simple optical targets that can be easily setup or optionally selected by the system itself using fixed landmarks that already exist in the environment in which the object such as a robot is operating. Such a system is straightforward to implement and allows real time six degree of freedom (6DOF) tracking of position and orientation of the object to be achieved for a fraction of the cost of existing systems. In this regard, prior systems typically use a laser tracker positioned in the environment which tracks a target on the object and relies on a laser beam having an uninterrupted line of sight to the target. Laser trackers have moving components and require precise alignment and calibration on a regular basis. They are relatively delicate instruments. They require particular care when being used on construction sites and when being transported. A laser tracker unit must be set up on firm ground when used on a construction site. If line of sight is lost or if the laser beam is broken for any reason, a laser tracker based system loses control of the object being tracked. The above described system provides flexibility in being able to select the optimal number of cameras and/or targets in order to be able to ensure uninterrupted tracking of the object as it moves through the environment as well as ensuring that with appropriate camera/target spacing the triangulation problem remains well conditioned.

A number of further features will now be described.

Typically, the system includes a body attachable to the object, the body having a camera array including a plurality of spaced apart cameras each having a field of view with a central axis, with the central axis of adjacent spaced apart cameras being divergently spaced by a predetermined fixed angle. In one example, the camera array is arranged to be as compact as possible whilst in other examples it may be advantageous to spread the cameras out in a distributed manner around the object (for example to improve line sight in some situations).

In one example, the fields of view of adjacent cameras of the camera array are at least partially overlapping so that at least some cameras can each view one or more common targets. This enables triangulation to be performed based on the known distance between the cameras and angular data to the target based on the pixel array coordinates.

In one example, the plurality of cameras (e.g. two or three spaced apart cameras) are spaced radially apart with their central axes lying in a common plane. In this arrangement, the fields of view of adjacent cameras are partially overlapping so that their fields of view intersect at a useable angle away from said common plane. Such a planar configuration of cameras would be of particular use for example in a system that maintains a substantially horizontal orientation of the radial camera array. Any suitable number of cameras may be included in the camera array (examples of which will be described below). Typically, the number of cameras in the camera array is selected from: <NUM>, <NUM>, <NUM>, <NUM>, <NUM>, <NUM>, <NUM>, <NUM>, <NUM>, <NUM>, <NUM>, <NUM>, <NUM>, <NUM>, <NUM>, <NUM>, <NUM>,<NUM> and <NUM>.

In another example, the body is spherical and the camera array includes a plurality of cameras arranged about the spherical body with their central axes spaced at predetermined angles. Such an arrangement would be of particular use when the camera array can have any arbitrary orientation.

It is to be appreciated that in this description a target may define either an individual point target or a target array comprising a plurality of point targets and furthermore that the plurality of spaced apart targets may be either user or computer generated. A user generated target is taken to include targets that are manually setup in the environment at predetermined locations by the user. A computer generated target is taken to include targets that are selected by the processor in accordance with fixed reference points that exist in the environment that the at least one camera is able to view. The targets may be selected by appropriate artificial intelligence or machine learning algorithms which are able to discern targets and make decisions around particular targets that should be chosen for use by the tracking system.

Referring now to user generated targets, the plurality of spaced apart targets may include a plurality of target arrays of predetermined configuration, each of the target arrays having at least three targets spaced apart from each other by predetermined distances. Typically, a target array has a unique spaced apart configuration of targets, compared with any other target array which assists the processor in identifying which particular target is being imaged.

A target array may take many forms including for example an upright mast on which the at least three targets are disposed in a spaced apart and colinear arrangement and a cross having upright and horizontal members each supporting a plurality of targets disposed in a spaced apart and colinear arrangement. In the example of a cross configuration of targets, a single camera imaging this array of targets would be able to determine its position and orientation based on the relative appearance of the targets (e.g. apparent distance between targets) as the camera moves closer/further away and rotates in yaw, pitch and roll relative to the targets. In one example, the targets are arranged on each mast or cross spaced apart from each other according to unique predetermined target spacings for each mast or cross.

Typically, the plurality of spaced apart targets include at least two target arrays manually locatable in predetermined spaced apart positions, viewable by the camera array when the object is located in an operating position in which its position and orientation is to be determined.

In another arrangement, the plurality of spaced apart targets are arranged in configurations selected from one or both of spaced apart along a straight line; and, spaced apart in two intersecting lines. In this example, the or each line of targets is horizontally disposed and typically one camera views targets in the first line and a second camera views targets in the second line.

Other techniques for assisting in the unique identification of targets may be employed. For example colour may be used to distinguish the targets. The targets may comprise ultra-bright light emitting diodes. In other preferred arrangements, entire target arrays may be coloured identically, while separate target arrays have targets coloured identically but in different colours.

In another example, time synchronisation of targets is used. In this example, the targets are lights such as light emitting diodes (LEDs) that are time synchronised to switch on and off at defined intervals to thereby allow a camera imaging a target to identify the target that is imaged. The processor can determine that for an image taken at a particular time, at that time, a particular target was programmed to be on which thereby identifies which target is in the image. Multiple cameras may each see the target that is on at the same time. In practice, the time interval would be in the order of milliseconds and to the human eye it would likely appear that all of the targets were on. If the targets are time synchronised the design of the target arrays is simplified and cost and setup time would be reduced.

In another example of the system, the body includes a surveying target such as an SMR (Spherical Mount Reflector) that is viewable by a surveying instrument to determine the position of the body relative to the surveying instrument. The surveying instrument is preferably selected from one of a theodolite such as a total station, or a laser tracker, in which case the surveying target on the body will be an optical target or reflector. In such an example, the camera array is used to determine the orientation of the body or determines the orientation and position of the body to supplement the position data from the surveying instrument, or provides a back-up where the surveying target is occluded due to site activity and the surveying instrument is temporarily unable to provide distance data. Such an example may allow simpler point targets to be used for the orientation determination and position may be measured more accurately with the laser tracker, however it includes a laser tacker or similar which will add expense to the tracking system.

Referring now to examples in which the computer generates or determines its own targets. In one example, the at least one camera images the environment and the one or more electronic processing devices are further configured to receive the image data from the at least one camera; and, analyse the image data to identify a number of potential targets using image recognition algorithms; select a plurality of the potential targets for use by the tracking system based at least in part on a set of target rules; and, determine a position and orientation of the selected targets in the environment. As previously mentioned the targets are fixed reference points in the environment which may include for discernible parts of existing structures such as fences, roofs, buildings, trees, light posts etc..

Typically, the one or more electronic processing devices create a map of the environment including the selected targets. The map may be created during initial setup of the object in the environment, for example when the environment is being calibrated to setup a coordinate system in which the targets are positioned.

In some examples, a user can at least one of select the targets based on a number of potential targets identified by the one or more electronic processing devices; and, override, confirm or delete targets selected by the one or more electronic processing devices. In this way, a user can ultimately still have a degree of control over which targets are selected to ensure for example that a transient target such as a parked car or object which may move is not selected as a target.

As the object moves and the at least one camera images the environment, the one or more electronic processing devices are configured to analyse image data to identify at least some of the selected targets for use in position and/or orientation determination. Different targets would be selected as the object moves through the environment and better targets come into view of the at least one camera.

In one example, the system includes at least one initial reference target at a known location in the environment and wherein the one or more electronic processing devices are configured to determine the position and/or orientation of the object by imaging the at least one initial reference target; and, determine the position and/or orientation of one or more random targets at unknown locations in the environment using the determined position and/or orientation of the object. The initial reference target may be removed after a predefined number of random targets have been positioned in the environment by the one or more electronic processing devices. In this type of system, new targets can be established as the object moves through the environment. The above described system is based on a forward position calculation from the at least one camera to the targets and then a backward position calculation from the targets to the at least one camera. If a known target is used as an initial reference, then the at least one camera can calculate its position and orientation (via the backward calculation) and then calculate the position of the "unknown" or random targets using the forward calculation. Once enough random targets have been measured, the initial reference target can be removed and the at least one camera will continue to know where it is (from the backward calculation from the random targets) and can establish the position of new random targets via the forward calculation, and by doing this continuously or at intervals as it moves, can continue to establish new targets as it moves into new regions.

Typically, a target position in an image is determined by the one or more electronic processing devices analysing pixel target saturation, to determine pixel array coordinates for the centre of the target.

It will be appreciated that triangulation is performed on the processed image data to determine at least the distance between a target and a camera and that was previously described the system is configured to ensure that the triangulation problem is always well conditioned to provide distance accuracy.

Preferably, the pixel array coordinates corresponding to the position of a target are determined to sub-pixel resolution using any suitable sub-pixel resolution algorithm. The pixel array coordinates corresponding to the position of the target in the image are indicative of angular data representing a target heading angle and an elevation angle. This angular data is used in the triangulation to determine the position and orientation of the at least one camera.

In some instances it is also preferable to determine the velocity of the object in addition to its position and orientation. In one example, previously stored images are analysed by the one or more electronic processing devices to determine a rate of change of the target heading angle and target elevation angle for use in determining the velocity of the object.

It will be appreciated that the system also typically includes a look-up table of calibration data stored in memory of the one or more electronic processing devices, the calibration data including pixel position values and range correlated to camera focussing data, so that observed target pixel array coordinates have camera focussing data applied to thereby apply range correction in the determination of distance to targets. This enables the pixel array coordinates to be corrected for lens distortion and camera errors.

The camera used in the system is typically a digital camera having one of a charge-coupled device (CCD) image sensor; and, a complementary metal oxide semiconductor (CMOS) image sensor. Currently available high speed CMOS sensors can provide multi mega pixel images at high frame rates. For example the Alexima AM41 sensor can provide <NUM> x <NUM> pixels at <NUM> frames per second (fps). Sub pixel resolution algorithms calculate target position to approximately one tenth of a pixel. In an example using <NUM> cameras to obtain <NUM> view angle, the sub pixel horizontal resolution is <NUM> x <NUM> x <NUM> / <NUM> = <NUM> sub pixels per degree or <NUM> degrees. The sensors and optics in the camera array can be calibrated by imaging known points, or a grid, in multiple orientations of the camera array and applying a mathematical table of corrections. Thus each camera pixel coordinate can be mapped to a calibrated actual angle.

Preferably, the or each camera is connected to a dedicated image processor for processing the image data from the camera. The image processor is typically a Field Programmable Gate Array (FPGA) or an Application Specific Integrated Circuit (ASIC) but in other examples could be a microprocessor or a microcontroller.

The image processor of each camera sends data via a data bus to a dedicated position and velocity processor that determines the position, orientation and velocity of the object. The position and velocity processor is typically a Field Programmable Gate Array (FPGA) or an Application Specific Integrated Circuit (ASIC) but in other examples could be a microprocessor or a microcontroller. The position and velocity processor typically outputs data to a control and logging system via a fieldbus link such as Ethercat to enable the data to be used in the positional control of the robot end effector so that it can perform an interaction in the environment.

In another broad form, the present invention provides a method of tracking the position and/or orientation of an object in an environment, the method including mounting at least one camera to the object so that at least some of a plurality of targets are viewable by the at least one camera when the object is located in an operating position in which its position and/or orientation is to be determined; and, the method further including in one or more electronic processing devices: determining target position data indicative of the relative spatial position of the targets; receiving image data indicative of an image from the at least one camera, said image including at least some of the targets; and, processing the image data to: identify one or more targets in the image; determine pixel array coordinates corresponding to a position of the one or more targets in the image; and, using the processed image data to determine the position and/or orientation of the object by triangulation.

Typically, the method includes mounting a camera array to the object, the camera array including at least two cameras that are at least one of housed in a body attached to the object; and, distributed around the object.

Referring to <FIG>, a six degrees of freedom tracking system <NUM> is illustrated. The six degrees of freedom tracking system <NUM> is designed to track the position and orientation of an object in space. The six degrees of freedom tracking system <NUM> tracks the position of the object in three dimensions returning coordinate values for x and z coordinates in a horizontal plane and a value for height coordinate y. The six degrees of freedom tracking system <NUM> also returns values for pitch, roll and yaw of the object. The present application for the six degrees of freedom tracking system <NUM> is tracking the position and orientation of a brick laying and adhesive applying head within the confines of a building site, and is used to assist with the control of a brick laying robot in the construction of a building such as a house on a slab <NUM>. It will be appreciated that this example is not intended to be limiting.

The six degrees of freedom tracking system <NUM> is provided with a body <NUM>, provided with twenty cameras located radially in a horizontal plane to form a camera array, one of the cameras of the array being is indicated at <NUM>. The cameras collectively view three target arrays <NUM>, <NUM>, <NUM>. In practice each camera has a narrow field of view and each target will normally be visible to a single camera in the camera array <NUM>, and at the most, two cameras in the camera array <NUM>. Where the target concerned is visible to two cameras it will typically be due to orientation of the body <NUM> in a position where target is located in the overlapping fields of view of two adjacent cameras.

<FIG> shows the first embodiment of the camera array showing the body <NUM> as an over-square cylinder with seven of the twenty cameras in this embodiment being visible, being cameras indicated at <NUM>, <NUM>, <NUM>, <NUM>, <NUM>, <NUM> and <NUM>. The cameras in the camera array are arranged radially around a central axis <NUM>. This camera array with its horizontal configuration is optimised for a system that maintains a substantially horizontal orientation of the radial camera array, with axis <NUM> remaining substantially vertical. Pitch, roll and yaw of the object will be detected by the camera array and can be corrected for in the control of the orientation of the object to which the camera array is affixed. Further detail of the first embodiment of the camera array is shown in <FIG>, <FIG> and <FIG>.

<FIG> shows a second alternative embodiment of a camera array, provided as an alternative camera array to that shown in <FIG>. The second embodiment of the camera array has a spherical body <NUM> with cameras <NUM> arranged in a spherical pattern about a central point <NUM>. The camera array of the second embodiment is used in a tracking system that can have any arbitrary orientation.

Those skilled in the art will appreciate that the number and orientation of the cameras can be arranged to maintain line of sight and accommodate the structural requirements of the object being tracked. Positions for the cameras <NUM> on the spherical body <NUM> of the camera array can be determined using known algorithms.

<FIG> shows a transverse cross-section of the first embodiment of the camera array of the first embodiment. A first camera <NUM> is shown mounted to the body <NUM> and on the opposite side of the body <NUM> is mounted a second camera <NUM>. First camera <NUM> is connected to a first FPGA (field programmable gate array) <NUM> by a high speed data bus <NUM>. The second camera <NUM> is connected to a second FPGA <NUM> by second high speed data bus <NUM>. The first FPGA <NUM> and the second FPGA <NUM> are connected respectively to a 6DOF position and velocity FPGA module <NUM> by a first data bus <NUM> and a second data bus <NUM>. The individual FPGA's <NUM>, <NUM> and <NUM> and the data busses <NUM>, <NUM>, <NUM> and <NUM> could be implemented on a common PCB or set of pluggable PCBs or the FPGAs could exist as IP cores within a larger single FPGA. All of the devices could be combined into or implemented as ASICS (application specific integrated circuits).

The first camera <NUM> and the second camera <NUM> are representative of each camera in the camera array <NUM>, and each camera <NUM> is directly connected with a high speed bus <NUM> to an FPGA <NUM>. That is, in the camera array, each camera is connected by a high speed bus to a FPGA dedicated to that camera. All twenty of these FPGAs are connected to the 6DOF position and velocity FPGA module <NUM>.

<FIG> shows a target array <NUM>. The target array <NUM> has a structure <NUM> that supports targets <NUM>, <NUM>, <NUM>, <NUM>, <NUM> and <NUM>. The number of the targets <NUM> -<NUM> on the target array and the arrangement of the targets <NUM> - <NUM> on the target array can be varied without deviating from the concept and scope of the present invention. While the first embodiment illustrated in <FIG> shows three targets, increasing the number of target arrays <NUM> in the tracking system <NUM> increases both the system accuracy and the probability of line of sight from as many as possible of the total number of targets on the target arrays to the camera array.

The targets <NUM> - <NUM> are uniquely arranged on the target array <NUM> so that the distance between adjacent targets <NUM>-<NUM> is always unique. This unique coding of adjacent target <NUM>-<NUM> distance allows each target <NUM>-<NUM> to be uniquely identified. In the preferred embodiment of the target array <NUM>, the targets <NUM> - <NUM> are arranged in a substantially vertical pattern. Each target can be assigned a unique upper/lower ratio which is the distance to the target above it divided by the distance to the target below it, for example in <FIG>, target <NUM> has an upper/lower ratio = d2/d1. The upper/lower ratio for each target <NUM>-<NUM> uniquely identifies each target <NUM>-<NUM>. In addition to this, each target array <NUM>, <NUM> or <NUM> is different from the others by having the targets <NUM>-<NUM> spaced differently. This allows the target arrays <NUM>,<NUM> and <NUM> to be readily distinguished from each other.

Referring to <FIG>, in the preferred embodiment of the target array <NUM>, the structure <NUM> consists of a substantially vertical tube <NUM> supported by three legs <NUM>, <NUM> and <NUM> which rest on the ground <NUM>. In the preferred embodiment of the target array <NUM>, the structure <NUM> is made of carbon fibre reinforced plastic painted highly reflective white to minimise thermal distortion.

In the preferred embodiment of the target array <NUM>, the targets <NUM> to <NUM> are ultra-bright coloured LEDs. In the preferred embodiment of the camera <NUM>, the camera <NUM> is fitted with a filter <NUM> that passes the colour of the targets <NUM> - <NUM> and rejects other colours. In the preferred embodiment of the targets <NUM> to <NUM>, each target <NUM> is fitted with a combined lens, reflector and shield <NUM> that projects a light field of uniform shape that does not impact the structure <NUM> of the adjacent target array <NUM>.

<FIG> shows the camera array <NUM> located near an obstruction <NUM>. <FIG> shows that the camera <NUM> has a lowermost line of sight <NUM> that is obstructed by the obstruction <NUM>, and an upper most line of sight <NUM>. Since the lowermost line of sight <NUM> is obstructed by the obstruction <NUM>, target <NUM> is not visible to the camera <NUM>. In <FIG> the target <NUM>, being above the obstruction <NUM>, is visible to the camera <NUM>. Targets <NUM> and <NUM> are above the upper most line of sight <NUM> and are not visible to the camera <NUM>. Targets <NUM>, <NUM> and <NUM> are above the line of obstruction <NUM> and below the uppermost line of sight <NUM> and are therefore visible. The upper/lower ratio d2/d1 uniquely identifies target <NUM> and therefore all other visible targets <NUM> - <NUM> of the target array <NUM>. Even when the complete target array <NUM> is not visible to the camera <NUM>, the targets can be uniquely identified by their relative position and the upper/lower ratio, provided that at least three consecutive targets <NUM> - <NUM> are visible to the camera <NUM>.

<FIG> shows the data processing pipelines. In the preferred embodiment illustrated in <FIG> there are twenty cameras being "Camera <NUM>" <NUM>, "Camera <NUM>" <NUM>, "Camera <NUM>" to "Camera <NUM>" which are not shown, and "Camera <NUM>" <NUM>. Each camera has a similar data processing pipeline which will be illustrated by describing the camera data processing pipeline <NUM> for "Camera <NUM>" <NUM>. "Camera <NUM>" <NUM> is connected by a high speed data bus <NUM> to a camera FPGA analysis module <NUM>. The FPGA camera analysis module <NUM> contains programmed gate arrays whose purpose is in a first step <NUM> to identify the targets <NUM> to <NUM> shown in <FIG>. In a second step <NUM>, the images of the targets <NUM> to <NUM>, if present, of the targets <NUM>-<NUM> are identified by considering pixels with the correct values that are in the correct relationship with the surrounding pixels, and the targets identified in the previous photo. Thus pixel noise and false targets may be rejected. The target pixel coordinates of the centre of the target are then calculated by using industry standard machine vision gate array methods.

The "Camera <NUM>" <NUM> would have been previously calibrated using standard machine vision techniques. These follow a procedure where the camera takes a picture of the target. The pixel location is mapped to an A and B ray angle from a focal point known in 3D. The focal point is dependent upon the focus of the camera and also the pixel location; that is, the rays from the lens may be skew and not all originate from the same focal point. The aim of camera calibration is to determine for each pixel, the focus distance for each object distance, the 3D focal point coordinate and the A and B ray angles from the focal point coordinate. In a first instance, the calibration table consists of data for each individual pixel. In a second instance, the data set can be linearised so that a linear (or other deterministic function fit) best fit approximation is applied to the data between suitably spaced pixels. For example, lens distortion characteristics are such that the changes in calibration data can be considered to be linear across a small range, say (ten) pixels. This reduces the calibration data set size from say a <NUM> x <NUM> pixel array, to a calibration data set of say <NUM> x <NUM> (a <NUM> fold reduction in calibration data). Furthermore, this simplifies the calibration process, because rather than determining the A, B ray angles and focal data for each pixel, a calibration plate can be used which provides an X, Y target that is imaged onto a pixel, and via sub pixel resolution techniques, thereby to a sub pixel coordinate p, q. Each calibration plate target is imaged to a sub pixel resolution, with corresponding focal distance d data stored.

The measured data is then used to construct the calibration table at the desired calibration interval (e.g. ten pixels). These techniques are known and standard. The known process is extended by being repeated at different object (calibration plate) distances D by moving the calibration plate by a known amount. Multiple positions of the calibration plate then allow the A and B ray angle calibration data and the focal point coordinate to be determined for each calibration interval. This constructs a multi-dimensional look up table that for the calibration pixel p, q and object distance D, has calibration data (A, B ray angles, and x,y,z focal point coordinate.

This technique is further extended so that the calibration plate is imaged by multiple cameras. It may be further extended so that the calibration plate is a series of plates encircling the camera array. Alternatively the camera array may be mounted to a turntable that can be accurately moved through an angle, thereby presenting different cameras to view the calibration plate. Alternatively the camera array may be mounted on a tilting turntable that can be rotated and tilted accurately to present different cameras to view the calibration plate. The calibration process is automated to collect the required data.

The camera error corrections are then applied in a third step <NUM> resulting in data <NUM> consisting of the target heading angle and target elevation angle and also a rate of change of the target heading angle and a rate of change of the target elevation angle. The data <NUM> is transmitted on a data bus <NUM> to the 6DOF position and velocity FPGA module <NUM>. Each FPGA analysis module <NUM>, <NUM>, <NUM> and the seventeen others (not shown) is connected to the data bus <NUM> and passes the target heading and elevation angle data <NUM> to the 6DOF position and velocity analysis FPGA module <NUM>. As IC (integrated circuit) technology advances and FPGA and ASIC technology improves, it may be possible to combine the various FPGA modules shown into fewer modules, or perhaps even one module. With the state of the technology as at <NUM>, the preferred embodiment is to use an FPGA for each FPGA camera analysis module <NUM> and an FPGA for the 6DOF position and velocity FPGA module <NUM>.

The 6DOF position and velocity FPGA module <NUM> uses well known navigation and statistical algorithms to combine the multiple target heading angle, elevation angle and velocity data to calculate a single set of 6DOF position and velocity data <NUM> for the camera array <NUM>, which is transmitted by an internal data link <NUM> to a data link module <NUM> which formats and transmits the data to an external data link <NUM>.

In the preferred embodiment the data link <NUM> is an Ethercat fieldbus. In other embodiments the data link <NUM> could be an industrial field bus such as Modbus, RS232, Profibus, Sercos, Powerlink, RT Ethernet, UDP ethernet or in non-realtime applications TCPIP ethernet. In a further embodiment, the data link <NUM> could be a number of simulated encoder outputs in any type of encoder format such as quadrature, BiSS, Hiperface, Endat or as simulated resolver output. In less preferred embodiments, the data link <NUM> could be multiple analogue signals such as <NUM>-20mA current loop or +-10V analogue.

The datalink <NUM> connects the 6DOF tracking system <NUM> to a control and logging system <NUM>.

In some instances it will be most convenient for the camera array <NUM> to be as compact as possible. In other instances it may be necessary for reasons such as line of sight for the camera array <NUM>, to be spread out. In some situations, it may be advantageous for the camera array to be distributed around an object. <FIG> shows a distributed camera array <NUM> around an arbitrary object <NUM>. The ten cameras <NUM>-<NUM> are distributed around the object <NUM>. Each camera has a directly connected camera analysis module, not shown for clarity for cameras <NUM> and cameras <NUM>-<NUM>, but shown for camera <NUM> which is directly connected to camera analysis module <NUM>. Camera analysis module <NUM> includes a field bus connection <NUM>. The fieldbus connection <NUM> is connected by a first cable <NUM> to the fieldbus network <NUM> and by a second cable <NUM> to the fieldbus network <NUM> which includes the other cameras <NUM> and <NUM> to <NUM> and a control system <NUM>. Each camera <NUM> to <NUM> transmits via its respective camera analysis module, the target pixel coordinates and pixel velocity as numerical data to the fieldbus network <NUM>. In the preferred embodiment of the distributed camera array <NUM>, the fieldbus network <NUM> is an Ethercat network and the control system <NUM> is a Twincat master on an industrial PC. The control system <NUM> includes a software module to combine the camera data and calculate the 6DOF position and velocity.

<FIG> shows a preferred embodiment of the construction of the electronics of the camera array <NUM> shown previously in <FIG> and <FIG>. Each camera <NUM> has a CMOS or CCD IC <NUM> surface mounted to a first PCB <NUM>. First camera PCB <NUM> includes the FPGA camera analysis module <NUM>. Each camera PCB <NUM> is fitted by an edge connector <NUM> to a socket <NUM> surface mounted to a back plane PCB <NUM>. Each edge connector socket <NUM> has tracks <NUM> connecting it to the 6DOF position and velocity module <NUM> which is then connected to the fieldbus connectors <NUM> which in turn connect to the external data link <NUM>.

<FIG> shows a cross section of a first embodiment of the assembly of the electronics of the camera array <NUM> into a CNC machined billet <NUM>-T6 aluminium lower housing <NUM> and upper housing <NUM>. The back plane PCB <NUM> is screwed to multiple standoffs <NUM> which may be integrally machined with the lower housing <NUM>. Each camera PCB <NUM> is crewed to camera mount bosses <NUM> which may be integrally machined with the lower housing <NUM>. Lenses <NUM> are fitted in holes <NUM> and sealed with O rings <NUM>. The upper housing <NUM> is mechanically fastened (not shown) to the lower housing <NUM> and sealed with an O ring <NUM>.

<FIG> shows a cross section of a second embodiment of the assembly of the electronics of the camera array into a lower housing <NUM>, which also mounts the lenses <NUM> in holes <NUM> which are sealed with O rings <NUM>. A lid <NUM> is fastened to the lower housing <NUM> with screws <NUM> and sealed by an o ring <NUM>. The back plane PCB <NUM> is mounted to standoffs <NUM> that are integrally machined with the lower housing <NUM>, by screws <NUM>.

<FIG> shows an alternative embodiment with a first horizontal target array <NUM> and a second horizontal target array <NUM>, located normal to each other and spaced from two boundaries of a working envelope <NUM>. In this embodiment the camera array <NUM> has a minimal plurality of cameras, in this case a first camera <NUM> and a second camera <NUM>. The first camera <NUM> has lines of sight <NUM>, <NUM>, and the second camera <NUM> has lines of sight <NUM>, <NUM>. The first camera <NUM> must maintain vision of at least three targets on the first horizontal target array <NUM> and the second camera <NUM> must maintain vision of at least three targets on the second horizontal target array <NUM> for the embodiment <NUM> to be able to calculate its position and orientation. This defines the working envelope <NUM> of this embodiment. An advantage of this embodiment is that fewer cameras <NUM>, <NUM> are required than in the other embodiment of the camera array <NUM>. A disadvantage of this embodiment is that the working envelope <NUM> is reduced in area and the orientation of the camera array <NUM> must only vary within a small range so that the target arrays <NUM>, <NUM> are in view of the respective cameras <NUM>, <NUM>.

<FIG> shows an embodiment of the tracking system <NUM> in which three single targets <NUM>, <NUM>, <NUM> are placed around a working area <NUM>. A three axis laser tracker <NUM> is set up adjacent to the working area <NUM>. The camera array <NUM> supports a laser tracker target <NUM> such as an SMR (Spherical Mount Reflector). The laser tracker <NUM> tracks the 3D position of the laser tracker target <NUM>. No orientation data is available from the three axis laser tracker <NUM> because it measures only the position of the laser tracker target <NUM>. The camera array <NUM> is used to measure the orientation. During set up it is possible for the laser tracker <NUM> to measure the precise position of the targets <NUM>, <NUM> and <NUM>. Alternatively, once the camera array <NUM> has been moved to three different positions (not shown), the precise position of the targets <NUM>, <NUM>, <NUM> can be determined by known surveying or navigation calculations. This embodiment of the tracking system <NUM> has the advantage that it may measure position more accurately than the embodiment of the tracking system <NUM> but with the disadvantage that it adds an expensive laser tracker <NUM> and requires more set up work to set up the laser tracker <NUM>. The targets <NUM>, <NUM> and <NUM> are simpler than the target arrays <NUM>, <NUM>, <NUM>, <NUM>, <NUM>.

<FIG> shows an embodiment of the tracking system <NUM> in which three single targets <NUM>, <NUM>, <NUM> are placed around a working area <NUM>. For set up the camera array <NUM> is sequentially positioned at three precisely known positions and orientations <NUM>, <NUM> and <NUM> on a set up jig <NUM>. Each set up position <NUM>, <NUM> and <NUM> may be provided with a coded transducer, not shown to communicate with the 6DOF position and velocity FPGA module <NUM>, the current position, <NUM>, <NUM>, or <NUM> of the camera array on the set up jig <NUM>. The set up jig <NUM> provides a known linear scale to the triangular measurements taken of the targets <NUM>, <NUM> and <NUM> during set up at the known positions and orientations <NUM>, <NUM> and <NUM>. By known surveying or navigation calculations, the 3D positions of the targets <NUM>, <NUM>, <NUM> can be determined. The set up jig <NUM> can then be removed and the 6DOF tracking system <NUM> can then determine the position and orientation of the camera array <NUM>, relative to the fixed targets <NUM>, <NUM>, <NUM>. An advantage of embodiment <NUM> is that only simple targets <NUM>, <NUM>, <NUM> are required and no laser tracker <NUM> (see <FIG>) is required. The disadvantage is that the set up jig <NUM> must be set up and the camera array <NUM> must be moved to the three known positions <NUM>, <NUM>, <NUM>. It should be noted that the camera array <NUM> could be moved manually or could be moved by a motorised stage or robot not shown.

Referring now to <FIG>, there is shown a tracking system <NUM> including a camera array <NUM> mounted to an object <NUM> such as a robotic arm. In this example, the cameras of the camera array <NUM> determine the position and orientation of the object <NUM> via targets <NUM>, <NUM>, <NUM> that are fixed reference points in the environment. The targets, in this case part of a roof, fence and street lamp are targets that would be selected by the processing device of the system as being suitable targets for use by the system. The targets would be imaged in an initial image capture of the environment and then suitable machine learning or artificial intelligence algorithms would be used to identify objects in the image and select certain objects as being suitable for use as targets, for example, in accordance with predefined target rules. Such target rules might include for example not using transient objects as targets that are likely to move either in the environment or entirely out of the environment. For example, items like cars and lawnmowers or animals like birds or dogs (or people) should not be used as targets whereas static objects like roofs, windows or other structural parts of buildings would be suitable. The system would therefore be able to perform image processing to discern what objects are in the image and then select suitable objects for use as targets (discernible parts thereof such as corners for example).

The system <NUM> would then generate a map of the environment during initial calibration of the robot in the environment to position the computer generated targets in the environment. As the object moves throughout the environment, the camera array images some of the selected targets for reference points used in the determination of the position and orientation of the object. Such a system is advantageous as it does not require targets to be manually positioned in the environment by a user and does not require any special type of target design as the targets already exist in the environment in which the object such as a robot is operating.

Referring now to <FIG>, a method of tracking the position and/or orientation of an object in an environment shall now be described.

In this example, at step <NUM>, the method includes mounting at least one camera to the object so that at least some of a plurality of targets are viewable by the at least one camera when the object is located in an operating position in which its position and/or orientation is to be determined.

At step <NUM> the method includes optionally positioning the targets in the environment, for example when the targets are manually placed by a user such as target arrays and the like. Alternatively, the system is able to deduce its own targets which pre-exist in the environment using artificial intelligence or other type of image recognition algorithm.

At step <NUM>, the method includes in one or more electronic processing devices determining target position data indicative of the relative spatial position of the targets. This data comprising the relative spatial position of the targets may be manually input to a processing device by a programming interface such as a keypad or may be provided as data measured by surveying equipment such as a total station used to survey the environment (e.g. a building site) once the targets have been placed. Alternatively, the target position data may be determined by creating a cartesian map of the environment with computer selected targets (such as fixed landmarks or reference points in the environment) forming part of the map and their positions determined relative to an environment coordinate system assigned to the map.

At step <NUM>, the method includes in the one or more electronic processing devices receiving image data indicative of an image from the at least one camera, said image including at least some of the targets. At step <NUM>, the image data is processed to identify one or more targets in the image and determine pixel array coordinates corresponding to a position of the one or more targets in the image at step <NUM>.

Finally, at step <NUM> the processed image data is used to determine the position and/or orientation of the object by triangulation.

Accordingly, it will be appreciated that in at least one example the above described tracking system provides a useful alternative to known tracking systems that is cheaper to implement, as it provides a camera based alternative to a more common laser tracker based measurement system. The system preferably uses an array of cameras imaging a plurality of sufficiently spaced apart targets in the environment to ensure that the triangulation problem is always well conditioned so that location can be determined to a high accuracy.

Claim 1:
A tracking system (<NUM>) for tracking the position and/or orientation of an object (<NUM>) in an environment (E), the tracking system (<NUM>) including:
a) a body attachable to the object (<NUM>), the body having a camera array (<NUM>) including a plurality of spaced apart cameras each having a field of view with a central axis, with the central axis of adjacent spaced apart cameras being divergently spaced by a predetermined fixed angle;
b) a plurality of spaced apart targets (<NUM>, <NUM>, <NUM>), at least some of said targets (<NUM>, <NUM>, <NUM>) viewable by the at least one camera of the camera array (<NUM>) , wherein the fields of view of adjacent cameras are at least partially overlapping so that at least some cameras can each view one or more common targets (<NUM>, <NUM>, <NUM>); and,
c) one or more electronic processing devices (<NUM>, <NUM>) configured to:
i) determine target position data indicative of the relative spatial position of the targets (<NUM>, <NUM>, <NUM>);
ii) receive image data indicative of an image from the at least one camera, said image including at least some of the targets (<NUM>, <NUM>, <NUM>);
iii) process the image data to:
(<NUM>) identify one or more targets (<NUM>, <NUM>, <NUM>) in the image;
(<NUM>) determine pixel array coordinates corresponding to a position of the one or more targets (<NUM>, <NUM>, <NUM>) in the image;
iv) use the processed image data to determine the position and/or orientation of the object (<NUM>) by triangulation; and,
d) characterised in that
the object (<NUM>) is a robotic arm.