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 mitigated in textual programming
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:
| Size
|
Example 1
|
Example 2
| | --- | --- | --- | | 2x2 |
1 2 4 3
|
1 4 2 3
| | 3x3 | 1 2 3 6 5 4 7 8 9 | 1 6 7 2 5 8 3 4 9 | | 4x4 | 1 2 3 4 8 7 6 5 9 10 11 12 16 15 14 13 | 1 8 9 16 2 7 10 15 3 6 11 14 4 5 12 13
|
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)
| Parameter
|
Description
| | --- | --- | | index | Number of the point being learned. The first point has index 1, the second 2, and so on. The numbering of the individual points can be found in the table above. | | horstX | The X-position determined by horstFX. Can be obtained, for example, with getCurrentPose().x. | | horstY | The Y-position determined by horstFX. Can be obtained, for example, with getCurrentPose().y. | | horstZ | The Z-position determined by horstFX. Can be obtained, for example, with getCurrentPose().z. | | cameraX | The x-position determined by a camera. | | cameraY | The y-position determined by a camera. | | cameraZ | The z-position determined by a camera. |
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.
| Parameter | Description |
|---|---|
| Index | Number of the point being learned. The first point has index 1, the second 2, and so on. The numbering of the individual points can be found in the table above. |
| cameraX | The x-position determined by a camera. |
| cameraY | The y-position determined by a camera. |
| cameraZ | The z-position determined by a camera. |
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.
| Parameter
|
Description
| | --- | --- | | cameraX | The x-position determined by a camera. | | cameraY | The y-position determined by a camera. | | cameraZ | The z-position determined by a camera. |
The return value consists of an object that contains a position in x, y and z. This is the position that horst must move to in order to move to the point detected by the camera. The individual coordinates can be accessed with getX(), getY() and getZ().
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)This function initialises the cuboid by teaching the corner points individually. To do this, the first corner of the lower level of the cuboid is passed in a config map - either as a joint configuration or as a pose (xyz + Euler angle). Further parameters are x, y and z - coordinates of a 3D vector, which then completely defines the cuboid: the other corners are approached using the values defined in the vector (relative to the starting position/viewed from the first corner).
Parameters | Description |
|---|---|
| joints | Joint configuration of the first corner of the lower plane of the cuboid |
| or | |
| xyz+euler | Pose of the first corner of the lower plane of the cuboid |
| x | x-value of the vector with which the other corners are approached |
| y | y-value of the vector with which the other corners are approached |
| z | z-value of the vector with which the other corners are approacheden |
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 program 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.
| Parameters
|
Description
| | --- | --- | | E1K | [x, y, z] Sensor coordinates of Corner 1 (1st point of the lower face of the cuboid) | | E2K | [x, y, z] Sensor coordinates of Corner 2 (2nd point of the lower face of the cuboid) | | E3K | [x, y, z] Sensor coordinates of Corner 3 (3rd point of the lower face of the cuboid) | | E4K | [x, y, z] Sensor coordinates of Corner 4 (4th point of the lower face of the cuboid) | | E5K | [x, y, z] Sensor coordinates of Corner 5 (1st point of the upper face of the cuboid) | | E6K | [x, y, z] Sensor coordinates of Corner 6 (2nd point of the upper face of the cuboid) | | E7K | [x, y, z] Sensor coordinates of Corner 7 (3rd point of the upper face of the cuboid) | | E8K | [x, y, z] Sensor coordinates of Corner 8 (4th point of the upper face of the cuboid) | | E1R | [x, y, z] Robot position of Corner 1 (1st point of the lower face of the cuboid) | | E2R | [x, y, z] Robot position of Corner 2 (2nd point of the lower face of the cuboid) | | E3R | [x, y, z] Robot position of Corner 3 (3rd point of the lower face of the cuboid) | | E4R | [x, y, z] Robot position of Corner 4 (4th point of the lower face of the cuboid) | | E5R | [x, y, z] Robot position of Corner 5 (1st point of the upper face of the cuboid) | | E6R | [x, y, z] Robot position of Corner 6 (2nd point of the upper face of the cuboid) | | E7R | [x, y, z] Robot position of Corner 7 (3rd point of the upper face of the cuboid) | | E8R | [x, y, z] Robot position of Corner 8 (4th point of the upper face of the cuboid) |
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.
| Parameter | Description |
|---|---|
| index | Index of the cuboid to be used in correction |
| x | x-coordinate of the sensor position to be corrected |
| y | y-coordinate of the sensor position to be corrected |
| z | z-coordinate of the sensor position 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())