Optimisation absolute accuracy

This article describes how the deviation of a position determined by an external sensor (such as a camera) from the actual position of the robot can be minimised in textual programming.
This applies in particular to the Horst600. If a point is to be approached after calibration with a camera, the deviation of the robot from the position to be reached is up to 1.5 mm. This is too much for some applications. These inaccuracies exist for mechanical reasons, but can be minimised by the software.

Two different functions have been implemented for this purpose.

1 CameraAlignment

There are two functions that are required to minimise the deviation in absolute accuracy. These are teachCameraAlignment and getCorrectedDestination.

1.1 teachCameraAlignment

Firstly, the deviation must be determined at several positions. Several points must be specified for this. Both the position of the robot and the position determined by the camera must be specified.

A square number of points must be taught in. Between 2x2 and 10x10 points can be taught. The more points that are taught, the better the minimisation.

The points must be learnt in an S-shape and the (inner) squares that are formed must be as right-angled as possible.

Here are a few examples of how the points must be taught:

The teachCameraAlignment function has 7 or 4 transfer parameters. The variant with 7 transfer parameters is described first.

teachCameraAlignment(index, horstX, horstY, horstZ, cameraX, cameraY, cameraZ)

1.1.2. teachCameraAlignment(index, cameraX, cameraY, cameraZ)

This method is identical to the one above. Only the values for horst are missing. These are determined automatically from the current robot position.

1.2 getCorrectedDestination

If you have taught in a square number of points via teachCameraAlignment, you can now use these to minimise the deviation.
The getCorrectedDestination function is available for this purpose. The position of an object determined by the camera is expected as a transfer parameter here.

1.3. Example

//here is an example of how the functionality works in a 2x2 grid


//the points must be taught in clockwise or anti-clockwise, e.g.


// 1 2 or 1 4


// 4 3 2 3




//If 3x3 points are taught, they must be taught in an S-shape,


//e.g.


// 1 2 3 1 6 7


// 6 5 4 or 2 5 8


// 7 8 9 3 4 9




//TODO move to object in corner 1


moveJoint(0.25,0.0,0.225, 0,0, 1, 0);


var corner1Horst = getCurrentPose();


//TODO Determine the position of the object with camera; here I simply assign 3 random values


var corner1Camera = { x: 0.26, y: 0.0, z:0.225};


teachCameraAlignment(1, corner1Horst.x , corner1Horst.y, corner1Horst.z,


                corner1Camera.x, corner1Camera.y, corner1Camera.z);


//TODO move to object in corner 2


moveJoint(0.30,0.0,0.225, 0,0, 1, 0);


var corner2Horst = getCurrentPose();


//TODO Determine the position of the object with camera; here I simply assign 3 random values


var corner2Camera = { x: 0.31, y: 0.0, z:0.225};


teachCameraAlignment(2, corner2Horst.x , corner2Horst.y, corner2Horst.z,


                corner2Camera.x, corner2Camera.y, corner2Camera.z);


//TODO move to object in corner 3


moveJoint(0.30,0.1,0.225, 0,0, 1, 0);


var corner3Horst = getCurrentPose();


//TODO Determine the position of the object with camera; here I simply assign 3 random values


var corner3Camera = { x: 0.31, y: 0.11, z:0.225};


teachCameraAlignment(3, corner3Horst.x , corner3Horst.y, corner3Horst.z,


                corner3Camera.x, corner3Camera.y, corner3Camera.z);


//TODO move to object in corner 2


moveJoint(0.25,0.1,0.225, 0,0, 1, 0);


var corner4Horst = getCurrentPose();


//TODO Determine the position of the object with camera; here I simply assign 3 random values


var corner4Camera = { x: 0.26, y: 0.11, z:0.25};


teachCameraAlignment(4, corner4Horst.x , corner4Horst.y, corner4Horst.z,


                corner4Camera.x, corner4Camera.y, corner4Camera.z);


//now the four corner points have been learnt and our algorithm can be used


//TODO camera recognise an object somewhere; here random values


var destination = { x: 0.27, y: 0.05, z:0.25, q0: 0, q1: 0, q2: 1, q3: 0};


//let the software calculate a more precise point


var correctedDestination = getCorrectedDestination(destination.x, destination.y, destination.z);


//move to the object and grab it


moveJoint(correctedDestination.getX(), correctedDestination.getY(), correctedDestination.getZ(),


          destination.q0, destination.q1, destination.q2, destination.q3);

2. CalibratedQuader

Using the functions described below, it is possible to minimise the error described in the introduction in a 3D space, which is defined here as a cuboid. First the cuboid is defined, then the corrected position within this cuboid can be determined.

The cuboid can be defined in two different ways. Either by teaching the individual corner points (teachCalibratedQuader) or by transferring a config map that contains all the required information (initialiseCalibratedQuader)

2.1. teachCalibratedQuader

int teachCalibratedQuader(Map<String, Object> configMap, double x, double y, double z)

When the function is executed, a corresponding information message first appears. The robot then first moves to the centre of the cuboid. The robot then moves to the first corner of the cuboid, stops there and the programme queries the value of the sensor coordinates at this position via user input. Once the data has been entered, the robot moves back to the centre of the cuboid. The process is repeated until all corners of the cuboid have been travelled to.

The function returns the index of the initialised cuboid. It is possible to define several cuboids. This index must be used when correcting the position with getXYZinQuader.

3.2. initializeCalibratedQuader

int initializeCalibratedQuader(Map<String, Object> configMap)

This function is used to initialise the cuboid by transferring the coordinates recorded by the sensor and the corresponding robot position in a config map.

The function returns the index of the initialised cuboid. It is possible to define several cuboids. This index must be used when correcting the position with getXYZinQuader.

3.3. getXYZinQuader

Vector3D getXYZinQuader(int index, double x, double y, double z)

This function can be used to determine the corrected position that the robot should move to in a previously defined cuboid. The parameters are the index of the respective cuboid and the determined sensor position that is to be corrected.

The function returns the corrected (error-minimised) position that the robot should move to in order to reach the position determined by the sensor as accurately as possible. The corresponding entries can be accessed with getX(), getY() and getZ().

2.4. Example

teachCalibratedQuader({"joints": [10.0, 10.0, 10.0, 10.0, 10.0, 10.0]}, 0.1, 0.1, 0.1)

teachCalibratedQuader({"xyz+euler": [ 0.3612, -0.0112,  0.3913,  148.186,   54.163,  154.569]},0.05, 0.1, 0.2)

var index;

index=initializeCalibratedQuader({

        "E1K" : [ 0, 0, 0],

        "E2K" : [ 0, 1, 0],

        "E3K" : [ 1, 1, 0],

        "E4K" : [ 1, 0, 0],

        "E5K" : [ 0, 0, 1],

        "E6K" : [ 0, 1, 1],

        "E7K" : [ 1, 1, 1],

        "E8K" : [ 1, 0, 1],

        "E1R" : [ 0, 0.01, 0.01],

        "E2R" : [ 0, 1.01, 0.01],

        "E3R" : [ 0.99, 1, 0],

        "E4R" : [ 0.99, 0, 0],

        "E5R" : [ 0, 0.01, 1.01],

        "E6R" : [ 0, 1.01, 1.01],

        "E7R" : [ 0.99, 1, 1],

        "E8R" : [ 0.99, 0, 1]

});

show_info(index);

var test=getXYZinQuader(index,0.5,0.5,0.5);

sleep(10000);

show_info("x="+test.getX()+" y="+test.getY()+" z="+test.getZ())