CN113359461B - Kinematics calibration method suitable for bionic eye system - Google Patents

Abstract

The invention provides a kinematic calibration method suitable for a bionic eye system, belonging to the technical field of bionic eyes. The method of the present invention establishes the three-dimensional positioning measurement error model of the bionic eye system by introducing the three-dimensional positioning measurement of the machine bionic eye system after the kinematic model error and the three-dimensional positioning measurement of the laser rangefinder, and groups the kinematic model errors, and uses Taylor expansion to The approximate analytical formula of the three-dimensional positioning measurement of the bionic eye system after introducing the kinematic model error is solved, and finally the kinematic error parameters in the introduced kinematic model error are identified and compensated to improve the accuracy of the three-dimensional positioning measurement of the bionic eye system. precision.

Description

A kinematic calibration method suitable for bionic eye system

technical field

The invention belongs to the technical field of bionic eyes, and in particular relates to a kinematic calibration method suitable for a bionic eye system.

Background technique

The kinematic model error in the bionic eye system will reduce the accuracy of 3D positioning measurement. The kinematic model error of bionic eye system mainly comes from machining error, assembly error and joint zero position error. Obtaining the kinematic error parameters in the kinematic model error of the bionic eye system through kinematic calibration and compensating the error is of great significance for improving the accuracy of the three-dimensional positioning measurement of the bionic eye system.

Three-dimensional positioning measurement refers to the process of obtaining the information of the target to be measured through the measuring equipment and analyzing the information, so as to obtain the three-dimensional coordinates of the target to be measured. Three-dimensional positioning measurement is usually divided into contact and non-contact methods. The contact method uses a specific instrument to quickly and directly measure the three-dimensional positioning of the target, which has the advantage of high measurement accuracy, but it can only be applied to the scene where the instrument can touch the target to be measured, and may cause damage to the target during measurement. The non-contact method refers to the method of performing three-dimensional positioning measurement on the target without touching the target to be measured. The measurement accuracy of the non-contact method is not as high as that of the contact method, but due to the above-mentioned limitations of the contact method, the application range of the non-contact method is wider than that of the contact method. Non-contact methods mainly include active and passive three-dimensional positioning measurement methods.

The active three-dimensional positioning measurement method refers to actively transmitting a controllable signal to the target to be measured, and realizing the three-dimensional positioning measurement of the target by analyzing the transmitted signal and the returned signal. The active three-dimensional positioning measurement method requires a special signal generation and control device, the measurement system is relatively complicated, and the cost required for the measurement is relatively high. Active three-dimensional measurement methods mainly include structured light method, laser scanning method, time of flight (Time of Flight, TOF) and so on. The passive three-dimensional positioning measurement method refers to directly relying on natural light sources, and realizing the three-dimensional positioning measurement of the target by analyzing the information in the image acquired by the camera. Compared with the active three-dimensional positioning measurement method, the passive three-dimensional positioning measurement method is relatively simple in operation and relatively low in cost, and can be applied in various complex environments. Passive three-dimensional positioning measurement methods can be divided into monocular vision method, binocular vision method and multi-eye vision method according to the number of cameras.

Robot kinematics calibration is of great significance for improving the positioning accuracy of robots. In most of the robot kinematics calibration researches carried out by domestic and foreign scholars, the positioning accuracy of the robot refers to the pose accuracy of the robot end effector. Due to the existence of factors such as machining errors, poor assembly, and joint zero position deviation, there are differences between the ideal end-effector pose of the robot and the actual end-effector pose, which will reduce the positioning accuracy of the robot. Robot kinematics calibration refers to the process of improving the positioning accuracy of the robot by modifying the kinematics control model without changing the hardware configuration of the robot. According to research, the error of geometric parameters is the main factor affecting the positioning accuracy of robots, so many scholars have carried out research on robot kinematics calibration for geometric parameters. Robot kinematics calibration is mainly divided into four steps: kinematics modeling, measurement, parameter identification and error compensation.

(1) Kinematic modeling

In 1955, Denavit et al. proposed the Denavit-Hartenberg (D-H) model, which is currently the most widely used robot kinematics model.

(2) Measurement

In the process of robot kinematics calibration, external measuring equipment is used to measure the pose of the end effector of the robot, and the measured pose of the end effector is transformed into the base coordinate system of the robot, and then parameter identification is performed. The accuracy of the measurement is directly determined The accuracy of parameter identification is improved. Commonly used external measurement devices include laser trackers, theodolites, three-coordinate measuring machines, ballbars, and visual measurement devices.

(3) Parameter identification

Parameter identification usually refers to the process of identifying the error of robot kinematic parameters by establishing the mapping relationship between the robot kinematic parameter error and the terminal pose error, and using the optimization algorithm, which is the core issue in the robot kinematic calibration method. The most commonly used optimization algorithm for parameter identification is the least squares method.

(4) Error compensation

Error compensation refers to compensating the identified kinematic parameter error to the robot kinematic parameters, so that the mapping relationship between the robot kinematic parameters and the terminal pose is more accurate, thereby improving the positioning accuracy of the robot. Currently commonly used error compensation methods include differential error compensation, joint space compensation, and real-time error compensation based on neural networks.

In the active binocular vision system of a robot with only a movable neck, because the two cameras are fixed, the visual information that can be obtained is limited, especially the objects that are very close to the binocular vision system cannot be perceived. The robot's active binocular vision system with two cameras and a movable neck is closer to the human visual system and can obtain more visual information. However, most of the existing active binocular vision systems for robots with two cameras and a movable neck cannot meet the requirements of light weight and miniaturization.

When most robot binocular vision systems realize 3D measurement, it is necessary to meet the condition that the relative pose between the two cameras remains unchanged. When the relative pose of the two cameras changes (for example, when performing three-dimensional positioning measurement on a moving target), it is necessary to re-calibrate the stereo extrinsic parameters. In the existing three-dimensional positioning measurement method of the robot binocular vision system, the stereo external parameter error and the parallax error between the matching imaging points in the two images will have a great influence on the result of the three-dimensional positioning measurement.

Factors such as machining errors, assembly errors, and joint zero position errors in the bionic eye system will reduce the accuracy of the three-dimensional positioning measurement of the bionic eye system. Most of the existing robot kinematics calibration methods are used to improve the positioning accuracy of the industrial robot end. Therefore, it is necessary to propose a new kinematic calibration method suitable for the bionic eye system, to identify and compensate the kinematic error parameters in the kinematic model error of the bionic eye system, so as to improve the accuracy of the three-dimensional positioning measurement of the bionic eye system.

Contents of the invention

Aiming at the deficiencies in the prior art, the present invention provides a kinematics calibration method suitable for the bionic eye system, which improves the accuracy of the three-dimensional positioning measurement of the bionic eye system.

The present invention achieves the above-mentioned technical purpose through the following technical means.

A kinematics calibration method suitable for bionic eye system. By introducing the three-dimensional positioning measurement of the machine bionic eye system and the three-dimensional positioning measurement of the laser rangefinder after introducing the kinematic model error, the three-dimensional positioning measurement error model of the bionic eye system is established, and then the motion Grouping the errors of the kinematic model, using Taylor expansion to solve the approximate analytical formula of the three-dimensional positioning measurement of the bionic eye system after introducing the kinematic model error, and finally using the nonlinear optimization algorithm to calculate the kinematic error parameters in the introduced kinematic model error Identification, and the error parameters obtained from the identification are compensated into the approximate analytical formula of the three-dimensional positioning measurement of the bionic eye system after introducing the kinematic model error.

Further, the three-dimensional positioning measurement error model of the bionic eye system is:

是世界坐标系到仿生眼O_N-X_NY_NZ_N坐标系的变换矩阵逆矩阵，

是空间点在仿生眼基坐标系O_N-X_NY_NZ_N下的坐标，

是利用激光测距仪与反射球配合测量的空间点在世界坐标系的坐标。in,

is the inverse matrix of the transformation matrix from the world coordinate system to the bionic eye O _N -X _N Y _N Z _N coordinate system,

is the coordinate of the space point in the bionic eye-based coordinate system O _N -X _N Y _N Z _N ,

It is the coordinate of the space point in the world coordinate system measured by the laser rangefinder and the reflective ball.

按照如下公式计算得到：Further, the

Calculated according to the following formula:

P′ _N = ^N T′ _Cl P′ _Cl (1)

^NT₂表示坐标系{2}相对于基坐标系O_N-X_NY_NZ_N的齐次变换矩阵，²T_3l表示坐标系{3l}相对于坐标系{2}的齐次变换矩阵，

表示存在偏差的坐标系

相对于坐标系{3l}的齐次变换矩阵，

表示坐标系{4}相对于存在偏差的坐标系

的齐次变换矩阵，

表示存在偏差的坐标系

相对于坐标系{4}的齐次变换矩阵，

表示坐标系{5}相对于存在偏差的坐标系

的齐次变换矩阵，

表示存在偏差的坐标系

相对于坐标系{5}的齐次变换矩阵，

表示仿生眼左眼相机坐标系相对于存在偏差的坐标系

的齐次变换矩阵；Among them, P′ _Cl is the coordinate of the target space point P in the left-eye camera coordinate system of the bionic eye after the error is introduced, and the left-eye camera coordinate system after the error is introduced is aligned with the base coordinate system O _N -X _N Y _N Z _N secondary transformation matrix

^N T ₂ represents the homogeneous transformation matrix of the coordinate system {2} relative to the base coordinate system O _N -X _N Y _N Z _N , ² T _3l represents the homogeneous transformation matrix of the coordinate system {3l} relative to the coordinate system {2} ,

Indicates the offset coordinate system

The homogeneous transformation matrix with respect to the coordinate system {3l},

Indicates that the coordinate system {4} is relative to the offset coordinate system

The homogeneous transformation matrix of ,

Indicates the offset coordinate system

The homogeneous transformation matrix with respect to the coordinate system {4},

Indicates that the coordinate system {5} is relative to the offset coordinate system

The homogeneous transformation matrix of ,

Indicates the offset coordinate system

The homogeneous transformation matrix with respect to the coordinate system {5},

Indicates that the coordinate system of the left eye camera of the bionic eye is deviated relative to the coordinate system

The homogeneous transformation matrix of ;

Express the formula (1) in a functional form:

防生眼左、右眼各关节角θ_i以及运动学模型误差δχ，其中i＝4，5，6，7。The input of the function is the pixel coordinates of the imaging point of the target space point P in the imaging plane of the left and right cameras of the bionic eye

The joint angle θ _i of the left and right eyes of the anti-health eye and the error δχ of the kinematic model, where i=4,5,6,7.

Furthermore, the δχ is a kinematic model error composed of 25 sets of error parameters, and there are 40 error parameters in total, specifically: the translation error parameters δx _3l of the coordinate system {3l} in the X, Y, and Z directions, δy _3l , δz _3l , the rotation error parameters δα _3l , δβ _3l , δγ _3l around the Z, Y, and X axes of the coordinate system {3l}, the translation error parameters δx of the coordinate system {4} in the x, Y, and Z directions ₄ , δy ₄ , δz ₄ , the rotation error parameters δα ₄ , δβ ₄ , δγ ₄ around the Z, Y, and X axes of the coordinate system {4}, and the translation errors of the coordinate system {5} in the X, Y, and Z directions Parameters δx ₅ , δy ₅ , δz ₅ , the rotation error parameters δα ₅ , δβ ₅ , δγ ₅ around the Z, Y, and X axes of the coordinate system {5}, the rotation error parameters of the coordinate system {3r} in the X, Y, and Z directions Translation error parameters δx _3r , δy _3r , δz _3r , rotation error parameters δα _3r , δβ _3r , δγ _3r around the Z, Y, and X axes of the coordinate system {3r}, and the coordinate system {6} in the X, Y, and Z directions The translation error parameters δx ₆ , δy ₆ , δz ₆ on , the rotation error parameters δα ₆ , δβ ₆ , δγ ₆ around the Z, Y, and X axes of the coordinate system {6}, and the coordinate system {7} in X, Y, Translation error parameters δx ₇ , δy 7 , δz ₇ in the Z direction, rotation error parameters δα ₇ , δβ ₇ , δγ ₇ around the Z, Y, and X axes of the coordinate system { ₅ }, δθ _i is the ith joint angle The zero error parameter of .

Further, the approximate analytical formula of the three-dimensional measurement of the bionic eye system after the error is introduced is solved by using Taylor expansion, specifically:

包含的误差参数，o(·)表示泰勒展开式的高阶小项。in:

Included error parameter, o( ) represents the higher-order minor term of the Taylor expansion.

Furthermore, the approximate analytical formula for the three-dimensional positioning measurement of the bionic eye system after the error is introduced is:

Furthermore, the non-linear optimization algorithm is used to identify the kinematic error parameters in the introduced kinematic model error, specifically: obtain the optimized transformation matrix ^N T _W through iteration, and identify the kinematic model error 40 kinematic error parameters of the bionic eye system in δχ.

Furthermore, the goal of optimizing the kinematics error model of the bionic eye system is to find the error δχ of the kinematics model of the left and right eyes of the bionic eye and the transformation matrix from the world coordinate system to the base coordinate system of the bionic eye O _N -X _N Y _N Z _N ^N T _W , making the error model the smallest, expressed as:

The beneficial effects of the present invention are:

(1) The present invention sets out from the structure and motion mechanism of human eyeball and neck, the visual mechanism of human eyeball, designs and develops the bionic eye system of binocular follow-up, establishes and verifies the kinematic model of bionic eye system, the designed The bionic eye system can meet the requirements of light weight and miniaturization, and has the characteristics of high flexibility and wide field of view.

(2) The present invention aims at the problem that the kinematic model error existing in the bionic eye system will reduce the accuracy of three-dimensional positioning measurement, and proposes a kinematic calibration method suitable for the bionic eye system. The error of the three-dimensional measurement is modeled, and the kinematic model errors are grouped, and the approximate analytical formula of the three-dimensional positioning measurement of the bionic eye system after introducing the kinematic model error is solved by using Taylor expansion, which greatly reduces the amount of calculation of the symbolic operation Finally, the kinematic error parameters in the introduced kinematic model error are identified and compensated to improve the accuracy of the three-dimensional positioning measurement of the bionic eye system.

Description of drawings

Fig. 1 is a flow chart of the kinematic calibration method applicable to the bionic eye system according to the present invention;

Fig. 2 is the physical figure of the high-precision absolute laser tracker used in the present invention;

Fig. 3 is the physical figure of the reflecting ball used in the present invention;

Fig. 4 is a schematic diagram of the coordinate system of the bionic eye system of the present invention.

detailed description

The present invention will be further described below in conjunction with the accompanying drawings and specific embodiments, but the protection scope of the present invention is not limited thereto.

The present invention is applicable to the kinematics calibration method of the bionic eye system. By introducing the three-dimensional positioning measurement of the machine bionic eye system and the three-dimensional positioning measurement of the laser rangefinder after introducing the kinematic model error, the three-dimensional positioning measurement error model of the machine bionic eye system is established, and then the Kinematic model errors are grouped, and Taylor expansion is used to solve the approximate analytical formula of the three-dimensional positioning measurement of the bionic eye system after the introduction of kinematic model errors. The identification is carried out, and the error parameters obtained from the identification are compensated to the approximate analytical formula of the three-dimensional positioning measurement of the bionic eye system after introducing the kinematic model error, so as to improve the accuracy of the three-dimensional positioning measurement of the bionic eye system.

As shown in Figure 1, the present invention is applicable to the kinematic calibration method of the bionic eye system, which specifically includes the following steps:

Step (1), kinematic error modeling of bionic eye system based on 3D positioning measurement

1) The ideal model for three-dimensional positioning measurement of the bionic eye system

The coordinate system of the bionic eye system defined based on the standard D-H method is shown in Figure 2.

The D-H parameters of the bionic eye system are shown in Table 1, and the specific meaning of each parameter in the D-H parameter table is explained as follows:

Joint distanced _i (connecting rod offset): along the z _i-1 axis, the distance from x _i-1 to x _i .

Joint angleθ _i (joint angle): along the z _i-1 axis, the angle from x _i-1 to x _i .

Link length a _i (link length): along the x _i axis, the distance from z _i-1 to z _i .

Link twist angleα _i (connecting rod rotation angle): along the x _i axis, the angle from z _i-1 to z _i .

q _i : the joint angle θ _i of each joint of the bionic eye in the initial pose.

The homogeneous transformation matrix between two adjacent joints in the bionic eye system established by using the DH parameters in Table 1 and formula (1) ( ⁱ - ¹ T _i is the homogeneous transformation matrix from joint i to joint i-1) As shown in formulas (2)-(9).

Table 1 D-H parameters of the bionic eye system

The stereo extrinsic parameters ^Cr T _Cl of the bionic eye system can be calculated as follows:

^Cr T _Cl ＝( ² T _3r ^3r T ₆ ⁶ T ₇ ⁷ T _Cr )- ¹ (2T _3l ^3l T ₄ ⁴ T ₅ ⁵ T _Cl ) (10)

Among them, ⁵ T _Cl and ⁷ T _Cr are the head-eye parameters of the left and right cameras of the bionic eye system after calibration, and the calibration results are as follows:

The internal parameter matrix of the left and right cameras of the bionic eye system is expressed as follows:

Distortion correction is performed on the left and right camera images of the bionic eye system. After distortion correction, the pixel coordinates of the imaging points of the target space point P in the left and right camera imaging planes are expressed as

Fix the world coordinate system O _W -X _W Y _W Z _W to the left eye camera coordinate system of the bionic eye. The coordinates of the target space point P in the world coordinate system are [x _W y _W z _W 1] ^T , the target space point P in the real world, after projection, falls on the physical imaging plane (pixel plane), and on the camera plane The coordinates of the imaging point p _C are [x _C y _C z _C 1] ^T (the coordinates of the imaging point p _Cl on the left-eye camera plane are [x _{C y Cl z Cl 1 ]} ^T , and on the right-eye camera plane The coordinates of the imaging point p _Cr are [x _cr y _Cr z _Cr 1] ^T ). The coordinates of the target space point P in the left and right eye camera coordinate systems are as follows:

The mapping relationship between the homogeneous coordinate [x _W y _W z _W 1] ^T of the target space point P in the world coordinate system and the homogeneous coordinate [uv 1] ^T of the imaging point p in the pixel coordinate system is as follows:

其中f为相机的焦距；将αf合并成f_u，将βf合并成f_v，在等式两边同乘z_C，并写成矩阵的表示形式：

其中

为相机的内参矩阵。相机的位姿由它的旋转矩阵R和平移向量t来描述，又称为相机的外参，则齐次变换矩阵

得到

式中隐含了一次齐坐标到非齐次坐标的转换(K·T)，为了式子能够成立，需要乘以转换矩阵[I_3×3|0]，其中，I_3×3为三维的单位矩阵。The pixel coordinate system is defined as: the origin is located in the upper left corner of the image, the u-axis is parallel to the right of the x-axis, and the v-axis is parallel to the y-axis downward. Between the pixel coordinate system and the imaging plane, there is a difference of one zoom and one translation of the origin. Assuming that the pixel coordinates are scaled by α times on the u-axis and β times on the v-axis, and at the same time, the origin is translated by [u ₀ v ₀ ] ^T , then the relationship between the coordinates of the imaging point p _C and the pixel coordinates [uv] ^T Formula is

Where f is the focal length of the camera; combine αf into f _u , combine βf into f _v , multiply z _C on both sides of the equation, and write it in the form of a matrix:

in

is the internal parameter matrix of the camera. The pose of the camera is described by its rotation matrix R and translation vector t, also known as the external parameters of the camera, then the homogeneous transformation matrix

get

The formula implies a transformation from homogeneous coordinates to non-homogeneous coordinates (K·T). In order for the formula to be established, it needs to be multiplied by the transformation matrix [I _3×3 |0], where I _3×3 is the three-dimensional identity matrix.

与点P在世界坐标系下的坐标[x_W y_W z_W 1]^T之间的关系如下：According to formula (17), the pixel coordinates of the imaging point of the target space point P in the imaging plane of the left camera can be obtained

The relationship with the coordinate [x _W y _W z _W 1] ^T of point P in the world coordinate system is as follows:

与点P在世界坐标系下的坐标[x_W y_W z_W 1]^T之间的关系如下：The pixel coordinates of the imaging point of the target space point P in the imaging plane of the right camera

The relationship with the coordinate [x _W y _W z _W 1] ^T of point P in the world coordinate system is as follows:

Eliminate Z _Cl and Z _Cr in formula (18) and formula (19) respectively, and then combine the two formulas to get:

Formula (20) is expressed in the form of AX=B. X = (A ^T A) ^-1 (A ^T B) can be used to solve the least squares solution of X, and the expression to obtain X is the coordinate P _Cl of the target space point P in the bionic eye left eye camera coordinate system .

Set the joints of the neck of the bionic eye to the initial state (the joint angle is at zero) and fix it, and establish the base coordinate system O _N -X _N Y _N Z _N at the end of the neck of the bionic eye. The three-dimensional coordinates of the target space point P in the base coordinate system O _N -X _N Y _N Z _N can be calculated as follows:

P _N = ^N T _Cl P _Cl (21)

where ^N T _Cl is expressed as the homogeneous transformation matrix of the left-eye camera coordinate system relative to the base coordinate system O _N -X _N Y _N Z _N , and ^N T _Cl can be calculated as follows:

^N T _Cl = ^N T ₂ ² T _3l ^3l T ₄ ⁴ T ₅ ⁵ T _Cl (22)

Among them, ^N T ₂ is expressed as the coordinate system of the end of the bionic eye neck (the homogeneous transformation matrix of the coordinate system {2} relative to the base coordinate system O _N -X _N Y _N Z _N ), expressed as follows:

2) Establishment of the three-dimensional positioning measurement model of the bionic eye system after introducing kinematic model errors

Since the bionic eye system is an active binocular vision system, its three-dimensional positioning measurement accuracy is largely affected by the error of the kinematic model. From the analysis of error sources, there are mainly two types of kinematic model errors in the bionic eye system:

仿生眼系统各坐标系的偏差可定义如下：①Due to the existence of machining errors and assembly errors, the coordinate system {3l}, {4}, {5}, {3r}, {6}, {7} of the bionic eye system shown in Figure 2 exists relative to the ideal position Deviation, the corresponding coordinate system with deviation is defined as

The deviation of each coordinate system of the bionic eye system can be defined as follows:

Where k=3l, 4, 5, 3r, 6, 7; Trans([δx _k δy _k δz _k ]) represents the translation error matrix, Rot _Z (δα _k ), Rot _Y (δβ _k ), Rot _X (δγ _k ) represents a rotation error matrix.

In formula (24):

Among them, δx _k , δy _k , and δz _k are the translation error parameters of the coordinate system {k} in the X, Y, and Z directions, respectively.

In the above formula, δα _k , δβ _k , and δγ _k are the rotation error parameters around the Z, Y, and X axes of the coordinate system {k}, respectively.

②Due to the existence of joint zero position error, the deviation of each joint angle θ _i (i=4, 5, 6, 7) of the left and right eyes of the bionic eye system can be defined as follows:

为存在偏差的关节角，δθ_i为第i个关节角的零位误差参数。in

is the joint angle with deviation, and δθ _i is the zero error parameter of the i-th joint angle.

According to formula (10), formula (24) and formula (29), it can be concluded that the stereo extrinsic parameters ^Cr T′ _Cl of the bionic eye system after introducing errors are as follows:

与点P在世界坐标系下的坐标[x_W y_W z_W 1]^T之间的关系仍如公式(18)所示。The pixel coordinates of the imaging point of the target space point P in the imaging plane of the left camera after the error is introduced

The relationship with the coordinate [x _W y _W z _W 1] ^T of point P in the world coordinate system is still as shown in formula (18).

与点P在世界坐标系下的坐标[x_W y_w z_W 1]^T之间的关系如下：The pixel coordinates of the imaging point of the target space point P in the imaging plane of the right camera after the error is introduced

The relationship with the coordinate [x _W y _w z _W 1] ^T of point P in the world coordinate system is as follows:

Eliminate Z _Cl and Z _Cr in formula (18) and formula (31) respectively, and then combine the two formulas to get:

Formula (32) is expressed in the form of A'X=B'. Also use X=(A' ^T A') ^-1 (A' ^T B') to solve the least squares solution of X, and obtain the expression of X as the target space point P in the bionic eye left eye camera after the error is introduced The coordinates P′ _Cl in the coordinate system.

After the error is introduced, the three-dimensional coordinates of the target space point P in the base coordinate system O _N -X _N Y _N Z _N can be calculated as follows:

P' _N = ^N T' _Cl P' _Cl (33)

where ^N _T'Cl can be calculated as follows:

Where ^N T ₂ is still represented by formula (23).

Formula (33) can be expressed in the following functional form:

仿生眼左、右眼各关节角θ_i(i＝4，5，6，7)以及运动学模型误差δχ，其中δχ是由25组误差参数组成的运动学模型误差，如表2所示。由表2可知，仿生眼左、右眼运动学模型误差δχ共包含40个待优化的误差参数。In formula (35), the input of the function is the pixel coordinates of the imaging point of the target space point P in the imaging plane of the left and right cameras of the bionic eye

The joint angles of the left and right eyes of the bionic eye θi ( _i =4, 5, 6, 7) and the kinematic model error δχ, where δχ is the kinematic model error composed of 25 sets of error parameters, as shown in Table 2. It can be seen from Table 2 that the error δχ of the kinematic model of the left and right eyes of the bionic eye contains a total of 40 error parameters to be optimized.

Table 2 The error δχ grouping of the kinematic model of the left and right eyes of the bionic eye

In summary, the 3D measurement method (algorithm 1) of the bionic eye system after introducing errors can be summarized as follows:

3) Error modeling based on 3D positioning measurement

仿生眼左、右眼各关节角θ_i(i＝4，5，6，7)以及运动学模型误差δχ相关，其中目标空间点P在仿生眼左、右相机成像平面中的成像点的像素坐标

仿生眼左、右眼各关节角θ_i(i＝4，5，6，7)已知，运动学模型误差δχ未知。运动学模型误差δχ会很大程度上影响仿生眼系统的三维定位测量精度，因此需要获取运动学模型误差以补偿三维定位测量误差。The three-dimensional coordinates P′ _N of the target space point P in the base coordinate system O _N -X _N Y _N Z _N and the pixel coordinates of the imaging point of the target space point P in the left and right camera imaging planes of the bionic eye

The joint angle θ _i (i=4, 5, 6, 7) of the left and right eyes of the bionic eye is related to the error δχ of the kinematic model, where the pixel of the imaging point of the target space point P in the imaging plane of the left and right cameras of the bionic eye coordinate

The joint angles θ _i (i=4, 5, 6, 7) of the left and right eyes of the bionic eye are known, and the error δχ of the kinematic model is unknown. The kinematic model error δχ will greatly affect the three-dimensional positioning measurement accuracy of the bionic eye system, so it is necessary to obtain the kinematic model error to compensate for the three-dimensional positioning measurement error.

世界坐标系固定在激光测距仪坐标系。根据算法1的流程计算这组空间点在仿生眼基坐标系O_N-X_NY_NZ_N下的三维坐标

世界坐标系到仿生眼O_N-X_NY_NZ_N坐标系的变换矩阵为^NT_W。仿生眼系统与激光测距仪对一组空间点进行三维定位测量的误差模型如下：Use the high-precision absolute laser rangefinder Leica AT960 (Figure 3) and the reflective ball (Figure 4) to measure the coordinates of a group (M) of space points in the world coordinate system

The world coordinate system is fixed in the laser rangefinder coordinate system. Calculate the three-dimensional coordinates of this group of space points in the bionic eye-based coordinate system O _N -X _N Y _N Z _N according to the process of Algorithm 1

The transformation matrix from the world coordinate system to the bionic eye O _N -X _N Y _N Z _N coordinate system is ^N T _W . The error model for the three-dimensional positioning measurement of a group of space points by the bionic eye system and the laser rangefinder is as follows:

Step (2) Identification and compensation of bionic eye system kinematics error parameters

1) Approximate analytical formula for 3D positioning measurement of the bionic eye system after introducing errors

As mentioned above, Algorithm 1 can be used to measure the three-dimensional coordinates P′ _N of the target space point P in the bionic eye-based coordinate system O _N -X _N Y _N Z _N after the error is introduced. Since the error δχ of the left and right eye kinematic models of the bionic eye contains a total of 40 error parameters to be optimized (Table 2), the amount of calculation is very large when using Algorithm 1 to perform the symbolic operation of _P'N . Therefore, Taylor expansion can be used to solve the approximate analytical formula of the three-dimensional measurement of the bionic eye system after the error is introduced.

According to Taylor expansion, there are:

In formula (37):

The error parameters included in δχ _k (k=1, 2,..., 25) are shown in Table 2; o(·) represents the high-order small term of the Taylor expansion.

Additionally, there are:

Formula (39) can be expressed as:

为输入运动学模型误差为

时求得目标空间点P在仿生眼基坐标系O_N-X_NY_NZ_N下的三维坐标P′_N。in

The input kinematic model error is

The three-dimensional coordinates P′ _N of the target space point P in the bionic eye-based coordinate system O _N -X _N Y _N Z _N are obtained at the same time.

Substituting formula (40) into formula (37), ignoring higher-order terms, we can get:

最多包含4个待优化的误差参数，利用公式(41)进行P′_N的符号运算时的运算量大大下降。本发明利用科学计算软件Mathematica对引入误差后的仿生眼系统三维定位测量的近似解析式进行求解。Formula (41) is the approximate analytical formula for the three-dimensional positioning measurement of the bionic eye system after the error is introduced. Compared with Algorithm 1, due to the approximate analytical formula

It contains at most 4 error parameters to be optimized, and the calculation amount when using the formula (41) to perform the symbolic operation of _P'N is greatly reduced. The invention uses the scientific calculation software Mathematica to solve the approximate analytical formula of the three-dimensional positioning measurement of the bionic eye system after the error is introduced.

In summary, the approximate analytical solution algorithm (Algorithm 2) for the three-dimensional positioning measurement of the bionic eye system after introducing errors can be summarized as follows:

2) Kinematic error parameter identification and compensation

能够尽可能的小，表示如下：The identification of kinematic error parameters of the bionic eye system refers to the process of identifying the kinematic error parameters of the bionic eye system by using a nonlinear optimization algorithm by establishing the mapping relationship between the kinematic error parameters of the bionic eye system and the three-dimensional measurement error. The goal of the present invention to optimize the kinematics error model of the bionic eye system is to find the error δχ of the kinematics model of the left and right eyes of the bionic eye and the transformation matrix ^N T from the world coordinate system to the base coordinate system of the bionic eye O _N -X _N Y _N Z _{N W} , such that the error model of equation (36)

can be as small as possible, expressed as follows:

的近似解析式可利用算法2进行求解。in

The approximate analytical formula of can be solved by Algorithm 2.

的近似解析式。公式(42)中^NT_W、δχ＝(δχ₁，δχ₂，...，δχ₂₅)^T为优化变量，在Ceres库中称为参数块。利用公式(42)构建代价函数，在Ceres库中称为残差块。然后可通过迭代获取优化后的变换矩阵^NT_W，并辨识出δχ中的40个仿生眼系统运动学误差参数。Use Google's nonlinear optimization library Ceres to solve the nonlinear optimization problem in formula (42). Using Mathematica to solve

an approximate analytical formula for . In formula (42), ^N T _W , δχ=(δχ ₁ , δχ ₂ , ..., δχ ₂₅ ) ^T are optimization variables, which are called parameter blocks in the Ceres library. The cost function is constructed using Equation (42), called a residual block in the Ceres library. Then the optimized transformation matrix ^N T _W can be obtained through iteration, and 40 kinematic error parameters of the bionic eye system in δχ can be identified.

对于设定的阈值ε，判断

和优化后的变换矩阵^NT_W代入误差模型e的表达式中后，是否能够满足e＜ε，若不满足则反复迭代，直至满足e＜ε为止，从而达到降低仿生眼系统三维定位测量误差的目的。Compensate (introduce) the 40 kinematic error parameters in the identified kinematic model error δχ of the bionic eye left and right eyes into formula (41), and get

For the set threshold ε, the judgment

After substituting the optimized transformation matrix ^N T _W into the expression of the error model e, whether it can satisfy e<ε, if not, iterate repeatedly until e<ε is satisfied, so as to reduce the three-dimensional positioning measurement error of the bionic eye system the goal of.

The described embodiment is a preferred implementation of the present invention, but the present invention is not limited to the above-mentioned implementation, without departing from the essence of the present invention, any obvious improvement, replacement or modification that those skilled in the art can make Modifications all belong to the protection scope of the present invention.

Claims (6)

1. A kinematic calibration method suitable for bionic eye systems, characterized in that, the three-dimensional positioning measurement of the bionic eye system by introducing the kinematic model error and the three-dimensional positioning measurement of the laser rangefinder are established Then, group the kinematic model errors into groups, use Taylor expansion to solve the approximate analytical formula of the three-dimensional positioning measurement of the bionic eye system after introducing the kinematic model errors, and finally use the nonlinear optimization algorithm to solve the kinematic model errors introduced Identify the kinematic error parameters, and compensate the identified error parameters into the approximate analytical formula of the three-dimensional positioning measurement of the bionic eye system after introducing the kinematic model error; The three-dimensional positioning measurement error model of the bionic eye system is:

in,

is the inverse matrix of the transformation matrix from the world coordinate system to the bionic eye O _N -X _N Y _N Z _N coordinate system,

is the coordinate of the space point in the bionic eye-based coordinate system O _N -X _N Y _N Z _N ,

It is the coordinate of the space point in the world coordinate system measured by the laser range finder and the reflective ball; The kinematic model error is a kinematic model error composed of 25 sets of error parameters, and there are 40 error parameters in total, specifically: the translation error parameters δx _3l , δy of the coordinate system {3l} in the X, Y, and Z directions _3l , δz _3l , the rotation error parameters δα _3l , δβ _3l , δγ _3l around the Z, Y, and X axes of the coordinate system {3l}, the translation error parameters δx ₄ of the coordinate system {4} in the X, Y, and Z directions _{. _ _ _ _} δx ₅ , δy ₅ , δz ₅ , the rotation error parameters δα ₅ , δβ ₅ , δγ ₅ around the Z, Y, and X axes of the coordinate system {5}, and the translation of the coordinate system {3r} in the X, Y, and Z directions Error parameters δx _3r , δy _3r , δz _3r , rotation error parameters δα _3r , δβ _3r , δγ _3r around the Z, Y, and X axes of the coordinate system {3r}, and the coordinate system {6} in the X, Y, and Z directions The translation error parameters δx ₆ , δy ₆ , δz ₆ , the rotation error parameters δα ₆ , δβ ₆ , δγ ₆ around the Z, Y, and X axes of the coordinate system {6}, and the coordinate system {7} in X, Y, and Z The translation error parameters δx ₇ , δy _{7 , δz 7 in the direction, the rotation error parameters δα 7 , δβ 7} , δγ ₇ around the Z, Y, and X axes of the coordinate system {7}, δθ _i is the i-th joint angle Zero position error parameters, where i=4,5,6,7. 2. the kinematic calibration method applicable to the bionic eye system according to claim 1, characterized in that, the

Calculated according to the following formula: P′ _N = ^N T′ _Cl P′ _Cl (1) Among them, P′ _Cl is the coordinate of the target space point P in the left-eye camera coordinate system of the bionic eye after the error is introduced, and the left-eye camera coordinate system after the error is introduced is aligned with the base coordinate system O _N -X _N Y _N Z _N secondary transformation matrix

^N T ₂ represents the homogeneous transformation matrix of the coordinate system {2} relative to the base coordinate system O _N -X _N Y _N Z _N , ² T _3l represents the homogeneous transformation matrix of the coordinate system {3l} relative to the coordinate system {2} ,

Indicates the offset coordinate system

The homogeneous transformation matrix with respect to the coordinate system {3l},

Indicates that the coordinate system {4} is relative to the offset coordinate system

The homogeneous transformation matrix of ,

Indicates the offset coordinate system

The homogeneous transformation matrix with respect to the coordinate system {4},

Indicates that the coordinate system {5} is relative to the offset coordinate system

The homogeneous transformation matrix of ,

Indicates the offset coordinate system

The homogeneous transformation matrix with respect to the coordinate system {5},

Indicates that the coordinate system of the left eye camera of the bionic eye is deviated relative to the coordinate system

The homogeneous transformation matrix of ; Express the formula (1) in a functional form:

The input of the function is the pixel coordinates of the imaging point of the target space point P in the imaging plane of the left and right cameras of the bionic eye

The joint angle θ _i of the left and right eyes of the bionic eye and the error δχ of the kinematic model. 3. The kinematics calibration method applicable to the bionic eye system according to claim 1, characterized in that, using Taylor expansion to solve the approximate analytical formula of the three-dimensional measurement of the bionic eye system after the error is introduced, specifically:

in:

δχ _k represents the error parameter, o(·) represents the high-order small term of the Taylor expansion, and δχ represents the error of the kinematic model, where k=1,2,...,25. 4. The kinematic calibration method applicable to the bionic eye system according to claim 3, wherein the approximate analytical formula for the three-dimensional positioning measurement of the bionic eye system after the introduction of the error is:

5. The kinematics calibration method applicable to the bionic eye system according to claim 1, wherein the kinematics error parameters in the introduced kinematics model error are identified with a nonlinear optimization algorithm, specifically: The optimized transformation matrix ^N T _W is obtained through iteration, and 40 kinematic error parameters of the bionic eye system in the kinematic model error δχ are identified. 6. The kinematic calibration method applicable to the bionic eye system according to claim 5, characterized in that, the goal of optimizing the kinematic error model of the bionic eye system is: to find the kinematic model error δχ and the world The transformation matrix ^N T _W from the coordinate system to the bionic eye-based coordinate system O _N -X _N Y _N Z _N minimizes the error model, expressed as:

Images