CN107553493A - A kind of robot kinematics' parameter calibration method based on displacement sensor for pull rope - Google Patents
A kind of robot kinematics' parameter calibration method based on displacement sensor for pull rope Download PDFInfo
- Publication number
- CN107553493A CN107553493A CN201710865454.9A CN201710865454A CN107553493A CN 107553493 A CN107553493 A CN 107553493A CN 201710865454 A CN201710865454 A CN 201710865454A CN 107553493 A CN107553493 A CN 107553493A
- Authority
- CN
- China
- Prior art keywords
- msub
- mtd
- mrow
- mtr
- delta
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
Landscapes
- Other Investigation Or Analysis Of Materials By Electrical Means (AREA)
Abstract
The invention discloses a kind of robot kinematics' parameter calibration method based on displacement sensor for pull rope, the differential error model of robot is established according to robot kinematics' model of foundation, obtains the constraint equation between robot end's position deviation and robot kinematics' parameter error；Robot basis coordinates system is determined, obtains the transformational relation between robot basis coordinates system and position measuring system coordinate system；Using position coordinates of the position measuring system robot measurement end being made up of displacement sensor for pull rope under position measuring system coordinate system, the position coordinates is converted into the position coordinates under robot basis coordinates system；The kinematics parameters deviation of least squares identification robot is used according to the deviation between the physical location of robot end and theoretical position；The actual value of robot kinematics' parameter is determined according to the theoretical value of this and robot kinematics' parameter.This method can effectively demarcate the kinematics parameters of robot, improve the absolute fix precision of industrial robot.
Description
Technical field
The present invention relates to Industrial Robot Technology field, particularly a kind of robot motion based on displacement sensor for pull rope
Learn parameter calibration method.
Background technology
Precision is to weigh the important indicator of industrial robot performance.The precision index of industrial robot includes resetting essence
Degree and absolute fix precision.The repetitive positioning accuracy of industrial robot is general very high (0.1mm or more preferable), absolute fix precision one
As relatively low (10mm or worse).With the rapid development of robot technology, industrial robot is also widely used to multiple fields
Industry spot, it is higher that the application of the technology such as laser cutting, visual servo and off-line programing is required for industrial robot to have
Absolute fix precision.Influence of the robot geometric parameter error to robot localization precision reaches more than 80%, robot motion
The kinematics parameters deviation of robot can be picked out by learning parameter calibration, according to the theory movement mould of deviation amendment robot
Type, effectively improve the absolute fix precision of robot.
More physical locations using laser tracker robot measurement end complete robot kinematics' parameter at this stage
Demarcation, expensive, the complex operation of laser tracker, volume is larger to be not easy to move.
The content of the invention
The technical problems to be solved by the invention are overcome the deficiencies in the prior art and provide and a kind of passed based on drawstring displacement
Robot kinematics' parameter calibration method of sensor, this method use drawstring position heredity sensor robot measurement end effector
Physical location, cost is low, installation and simple to operate, small volume, easy to remove, can effectively demarcate the kinematics of industrial robot
Parameter, improve the absolute fix precision of robot.
The present invention uses following technical scheme to solve above-mentioned technical problem：
According to a kind of robot kinematics' parameter calibration method based on displacement sensor for pull rope proposed by the present invention, including
Following steps：
S1, the MDH moulds for establishing according to the length of connecting rod of robot body, dead-center position and each joint direction of rotation robot
Type, the theoretical value of robot kinematics' parameter is determined, the differential error model of robot is established according to the MDH models of foundation, is obtained
Constraint equation between robot end's position deviation and robot kinematics' parameter error；
S2, the position measuring system formed using displacement sensor for pull rope determine robot basis coordinates system, obtain robot
Transformational relation between basis coordinates system and position measuring system coordinate system；
S3, using the position measuring system robot measurement end being made up of displacement sensor for pull rope in position measuring system
Position coordinates under coordinate system, the position coordinates is converted into the position coordinates under robot basis coordinates system, the position after conversion
Put the physical location that coordinate is robot end；
S4, the theoretical position of robot end obtained according to the MDH models of foundation, according to the physical location of robot end
In deviation and step S1 between theoretical position robot end's position deviation value and robot kinematics' parameter error it
Between constraint equation, use the kinematics parameters deviation of least squares identification robot；
It is S5, true according to the theoretical value of the obtained robot kinematics' parameter errors of step S4 and robot kinematics' parameter
Determine the actual value of robot kinematics' parameter.
Enter one as a kind of robot kinematics' parameter calibration method based on displacement sensor for pull rope of the present invention
Prioritization scheme is walked, the constraint equation in step S1 between robot end's position deviation and robot kinematics' parameter error：
Wherein, dnRepresent the deviation of robot end position, Ma、Mα、Md、Mθ、MβIt is robot kinematics' parameter error
Coefficient matrix, be 3 × n matrix, Δ a, Δ α, Δ d, Δ θ, Δ β represent the kinematics parameters deviation of robot, wherein Δ θ=
(Δθ1,Δθ2,...,Δθn), Δ d=(Δ d1,Δd2,...,Δdn), Δ a=(Δ a1,Δa2,...,Δan), Δ α=
(Δα1,Δα2,...,Δαn), Δ β=(Δ β1,Δβ2,...,Δβn), Δ ai、Δαi、Δdi、Δθi、ΔβiRepresent machine
The little deviation of people's the i-th articular kinesiology parameter, 1≤i≤n, n are the quantity of robot rotary articulation.
Enter one as a kind of robot kinematics' parameter calibration method based on displacement sensor for pull rope of the present invention
Prioritization scheme is walked, the constraint side between robot end's position deviation and robot kinematics' parameter error is obtained in step S1
Journey, comprise the following steps that：
S1.1：According to the MDH models of robot, the kinematic relation obtained between the adjacent two joint of robot is：
Wherein, ai、αi、di、θiAnd βiFor the kinematics parameters in the joint of robot i-th, 1≤i≤n, n are that robot rotation is closed
The quantity of section, c θiRepresent cos θi, s θiRepresent sin θi；
S1.2：Obtaining the error matrix between the adjacent two joint of robot to formula (1) both ends progress differential is：
Wherein, Δ ai、Δαi、Δdi、Δθi、ΔβiRepresent the little deviation of robot the i-th articular kinesiology parameter, orderConvolution (1), is obtained：
Formula (2) is transformed to according to formula (3)~(7)：
It is defined hereinForError matrix, so as to obtain：
Formula (3)~(7) are substituted into formula (9) and obtained：
Obtained according to formula (10)：
Wherein, diAnd δiRepresent respectivelySite error and attitude error；
S1.3：Error matrix between the adjacent two joint of robot is substituted into the transmission chain structure of robot, obtained：
Wherein,Position orientation relation of i-th joint relative to the i-th -1 joint is represented,Represent the i-th joint relative to
The position and attitude error in i-1 joints,For the minor variations of robot end's pose,The theoretical position of robot end is represented,
DefinitionUn+1For unit matrix, using Differential Principle, ignore higher differentiation item, obtain：
Wherein,It is Ui+1Inverse matrix；
Formula (1) and formula (10) are substituted into formula (14), obtain robot end's position deviation and robot motion
Learn the constraint equation between parameter error：
Wherein,WithMatrix U is represented respectivelyi+1The vector of four 3 × 1；dnRepresent robot end
The deviation of end position, Ma、Mα、Md、Mθ、MβIt is robot kinematics' ginseng for the 3 × n matrix obtained according to formula (15) abbreviation
The coefficient matrix of number deviation, Δ a, Δ α, Δ d, Δ θ, Δ β represent the kinematics parameters deviation of robot, wherein, Δ θ=(Δ
θ1,Δθ2,...,Δθn), Δ d=(Δ d1,Δd2,...,Δdn), Δ a=(Δ a1,Δa2,...,Δan), Δ α=(Δ
α1,Δα2,...,Δαn), Δ β=(Δ β1,Δβ2,...,Δβn)。
Enter one as a kind of robot kinematics' parameter calibration method based on displacement sensor for pull rope of the present invention
Walk prioritization scheme, Ma、Mα、Md、Mθ、MβIt is relevant with robot each joint angle angle value with the theory movement parameter of robot.
Enter one as a kind of robot kinematics' parameter calibration method based on displacement sensor for pull rope of the present invention
Prioritization scheme is walked, in the step S2, the range of the displacement sensor for pull rope used is 2000mm, linear precision 0.05%,
Repeatable accuracy is 0.02%, resolution ratio 0.0244mm, and position measuring system includes at least three displacement sensor for pull rope, installation
The pedestal of displacement sensor for pull rope and the actuator of carry drawstring.
Enter one as a kind of robot kinematics' parameter calibration method based on displacement sensor for pull rope of the present invention
Prioritization scheme is walked, in the step S2, determines that the basis coordinates system of robot comprises the following steps：
S2.1：Determine that the coordinate of robot basis coordinates system is former using the position measuring system being made up of displacement sensor for pull rope
Point and Z-direction；
S2.2：The Y-axis side of robot basis coordinates system is determined using the position measuring system being made up of displacement sensor for pull rope
To then determining the X-direction of robot using the right-hand rule.
Enter one as a kind of robot kinematics' parameter calibration method based on displacement sensor for pull rope of the present invention
Prioritization scheme is walked, in the step S3, according to the robot basis coordinates system determined in step S2, by position measuring system measurement
The position coordinates of robot end is converted to the position coordinates under robot basis coordinates system.
The present invention compared with prior art, has following technique effect using above technical scheme：The present invention uses drawstring
The end effector of robot composition measurement machine of displacement transducer, the pedestal of fixed pulling rope displacement transducer and carry drawstring
The position measuring system of people end physical location, by the physical location of the position measuring system robot measurement, robot
Theoretical position and the obtained theoretical position of robot of measurement substitute into the differential error model of robot, use least square method
The kinematics parameters deviation of robot is picked out, robot kinematics' parameter reality is worth to reference to robot kinematics parameters theory
Actual value.Compared to the method that robot kinematics' parameter calibration is carried out using laser tracker, the present invention is passed using drawstring displacement
The physical location of sensor robot measurement end, installation is simple and quick, and cost is low, small volume, and conveniently moving is simple to operate, can
To quickly finish robot kinematics' parameter calibration.
Brief description of the drawings
Fig. 1 is the flow chart that the present invention demarcates robot kinematics' parameter using displacement sensor for pull rope.
Fig. 2 is the position measuring system that the present invention is made up of displacement sensor for pull rope.
Fig. 3 is position measuring system sketch of the present invention.
Fig. 4 is present invention determine that the schematic diagram of robot basis coordinates system initial point position and Z-direction.
Fig. 5 is present invention determine that the schematic diagram of robot basis coordinates system Y direction.
Fig. 6 is that robot kinematics' parameter error of the present invention recognizes flow chart.
Embodiment
Technical scheme is described in further detail below in conjunction with the accompanying drawings：
Present embodiment discloses a kind of robot kinematics' parameter calibration method based on displacement sensor for pull rope,
Comprise the following steps：
S1：The MDH moulds of robot are established according to the length of connecting rod of robot body, dead-center position and each joint direction of rotation
Type, the theoretical value of robot kinematics' parameter is determined, the differential that robot is established according to robot kinematics' model of foundation misses
Differential mode type, obtain the constraint equation between robot end's position deviation and robot kinematics' parameter error；
S2：The position measuring system formed using displacement sensor for pull rope determines robot basis coordinates system, obtains robot
Transformational relation between basis coordinates system and position measuring system coordinate system；
S3：Using the position measuring system robot measurement end being made up of displacement sensor for pull rope in position measuring system
Position coordinates under coordinate system, the position coordinates is converted into the position coordinates under robot basis coordinates system, the position after conversion
Put the physical location that coordinate is robot end.
S4：The theoretical position of robot end can be obtained according to the theoretical MDH models of foundation, according to robot end's
Deviation between physical location and theoretical position uses the kinematics parameters deviation of least squares identification robot；
S5：The robot kinematics' parameter error and the theoretical value of robot kinematics' parameter obtained according to S4 determines machine
The actual value of device people's kinematics parameters.
Fig. 1 is robot kinematics' parameter calibration process schematic.Firstly the need of establishing a suitable robot motion
Model is learned, the present invention establishes the kinematics model of robot using MDH modeling methods.According to the robot MDH models of foundation, obtain
It is to the kinematic relation between the adjacent two joint of robot：
Wherein, ai、αi、di、θiAnd βiFor the kinematics parameters in the joint of robot i-th, c θiRepresent cos θi, s θiRepresent sin
θi；Obtaining the error matrix between the adjacent two joint of robot to formula (1) both ends progress differential is：
Δ a in formula (2)i、Δαi、Δdi、Δθi、ΔβiThe small inclined of robot the i-th articular kinesiology parameter is represented respectively
Difference, orderConvolution
(1), can obtain：
Formula (2) can be transformed to according to formula (3)~(7)：
It can define hereinForError matrix, so as to obtain：
Formula (3)~(7) are substituted into formula (9) and obtained：
It can be obtained according to formula (10)：
Wherein, diAnd δiRepresent respectivelySite error and attitude error.By between the adjacent two joint of robot
Error matrix is substituted into the transmission chain structure of robot, can be obtained：
In formula (13)Position orientation relation of i-th joint relative to the i-th -1 joint is represented,Represent the i-th joint relative to
The position and attitude error in the i-th -1 joint,For the minor variations of robot end's pose.1≤i≤n, n are robot rotary articulation
Quantity, definition It is Ui+1Inverse matrix, wherein Un+1For unit matrix, using Differential Principle, ignore
Higher differentiation item, it can obtain：
Formula (1) and formula (10) are substituted into formula (14), robot end's position deviation and robot can be obtained
Constraint equation between kinematics parameters deviation：
Wherein,WithMatrix U is represented respectivelyi+1The vector of four 3 × 1；dnRepresent robot end
The deviation of end position, Ma、Mα、Md、Mθ、MβIt is robot kinematics' ginseng for the 3 × n matrix obtained according to formula (15) abbreviation
The coefficient matrix of number deviation, Δ a, Δ α, Δ d, Δ θ, Δ β represent the kinematics parameters deviation of robot, wherein, Δ θ=(Δ
θ1,Δθ2,...,Δθn), Δ d=(Δ d1,Δd2,...,Δdn), Δ a=(Δ a1,Δa2,...,Δan), Δ α=(Δ
α1,Δα2,...,Δαn), Δ β=(Δ β1,Δβ2,...,Δβn)。
Robot location's measuring system is as shown in Fig. 2 execution by four displacement sensor for pull rope, pedestal and carry drawstring
Device forms.After displacement sensor for pull rope is fixed, between being measured respectively using displacement sensor for pull rope each it is relative away from
From then by the drawstring end carry of four displacement sensor for pull rope on actuator, according to the length of drawstring and pulling on displacement
Relative distance between sensor calculates position coordinates of the robot end relative to position measuring system coordinate system.
The simplified model of robot measurement terminal position as shown in figure 3, four displacement sensor for pull rope respectively positioned at A, B,
C, tetra- positions of D, respective pulling rope length are respectively la、lb、lc、ld, the establishment of coordinate system of position measuring system in A points, then
The position coordinates of robot end position T points is (x, y, z), and four displacement sensor for pull rope ends are away from robot end position T
Distance be r.From the figure 3, it may be seen that the Z axis of A point coordinates system overlaps perpendicular to ABCD planes, X-axis and AC, determined according to the right-hand rule
Y direction.
Angle between AT and AC is β, then：
If angle is α between AT and AD, the angle between AD and AC be γ then：
If angle is θ between AT and ABCD planes：
Z=(la+r)·sinθ (21)
Line length is then gone out according to displacement sensor for pull rope, carrying out calculating by above formula can show that robot end holds
The position coordinates (x, y, z) of row device is
Determine schematic diagram such as Fig. 4 and Fig. 5 of transformational relation between robot basis coordinates system and position measuring system coordinate system
Shown, Fig. 4 is to determine the schematic diagram of robot basis coordinates system initial point position and Z-direction, and Fig. 5 is to determine robot basis coordinates system
The schematic diagram of Y direction.The present invention establishes robot measuring basis coordinate system, robot base by the way of numerical fitting
The X/Y plane and position measuring system X/Y plane for marking system overlap.Origin position and the Z axis side of robot basis coordinates system are determined first
To：
1) it is (0,0,0,0, -90,0) to make each joint values of robot；
2) independent rotary machine person joint 1, is recorded a point every certain angle, is obtained using robot location's measuring system
Go out the coordinate value of series of points；
3) these points are fitted to a circular flat using the mode of numerical fitting；
4) axis of this circular flat is robot basis coordinates system Z axis, and the axis is put down with robot basis coordinates system XY
The intersection point in face is robot basis coordinates system initial point.
Determine robot basis coordinates system Y direction：
1) it is (0,0,0,0,0,0) to make each joint values of robot；
2) independent rotary machine person joint 2, is recorded a point every certain angle, is obtained using robot location's measuring system
Go out the coordinate value of series of points；
3) these points are fitted to a circular flat using the mode of numerical fitting；
4) axis of this circular flat is robot basis coordinates system Y-axis.
According to obtained robot basis coordinates system initial point position, Z axis and Y direction, robot is determined using the right-hand rule
The X-direction of basis coordinates system, the final basis coordinates system for determining robot.
Robot kinematics' parameter identification flow chart is as shown in Figure 6.Use position measuring system robot measurement end
Physical location, these measurement points should be distributed as evenly as possible in the working space of robot, according to the reality of robot end
Constraint side between the deviation and robot end's position deviation and robot kinematics' parameter error of position and theoretical position
Journey goes out the kinematics parameters deviation of robot using least squares identification.
It is final that the theoretical DH parameters of robot are compensated to obtain according to the robot kinematics' parameter error picked out
The actual DH parameters of robot, complete the kinematic calibration of robot.
Above content is to combine specific preferred embodiment further description made for the present invention, it is impossible to is assert
The specific implementation of the present invention is confined to these explanations.For general technical staff of the technical field of the invention,
On the premise of not departing from present inventive concept, some simple deductions can also be made or substituted, should all be considered as belonging to the present invention's
Protection domain.
Claims (7)
1. a kind of robot kinematics' parameter calibration method based on displacement sensor for pull rope, it is characterised in that including following step
Suddenly：
S1, the MDH models for establishing according to the length of connecting rod of robot body, dead-center position and each joint direction of rotation robot,
The theoretical value of robot kinematics' parameter is determined, the differential error model of robot is established according to the MDH models of foundation, obtains machine
Constraint equation between device people's terminal position deviation and robot kinematics' parameter error；
S2, the position measuring system formed using displacement sensor for pull rope determine robot basis coordinates system, obtain robot base
Transformational relation between mark system and position measuring system coordinate system；
S3, using the position measuring system robot measurement end being made up of displacement sensor for pull rope in position measuring system coordinate
Position coordinates under system, the position coordinates under robot basis coordinates system is converted to by the position coordinates, and the position after conversion is sat
Mark is the physical location of robot end；
S4, the theoretical position of robot end obtained according to the MDH models of foundation, according to the physical location and reason of robot end
By in the deviation between position and step S1 between robot end's position deviation value and robot kinematics' parameter error
Constraint equation, use the kinematics parameters deviation of least squares identification robot；
S5, the robot kinematics' parameter error obtained according to step S4 and robot kinematics' parameter theoretical value determine machine
The actual value of device people's kinematics parameters.
2. a kind of robot kinematics' parameter calibration method based on displacement sensor for pull rope according to claim 1, its
It is characterised by, the constraint equation in step S1 between robot end's position deviation and robot kinematics' parameter error：
<mrow>
<msup>
<mi>d</mi>
<mi>n</mi>
</msup>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mi>d</mi>
<mi>x</mi>
</mtd>
</mtr>
<mtr>
<mtd>
<mi>d</mi>
<mi>y</mi>
</mtd>
</mtr>
<mtr>
<mtd>
<mi>d</mi>
<mi>z</mi>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>=</mo>
<mo>&lsqb;</mo>
<mtable>
<mtr>
<mtd>
<msub>
<mi>M</mi>
<mi>a</mi>
</msub>
</mtd>
<mtd>
<msub>
<mi>M</mi>
<mi>&alpha;</mi>
</msub>
</mtd>
<mtd>
<msub>
<mi>M</mi>
<mi>d</mi>
</msub>
</mtd>
<mtd>
<msub>
<mi>M</mi>
<mi>&theta;</mi>
</msub>
</mtd>
<mtd>
<msub>
<mi>M</mi>
<mi>&beta;</mi>
</msub>
</mtd>
</mtr>
</mtable>
<mo>&rsqb;</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mi>&Delta;</mi>
<mi>a</mi>
</mtd>
</mtr>
<mtr>
<mtd>
<mi>&Delta;</mi>
<mi>&alpha;</mi>
</mtd>
</mtr>
<mtr>
<mtd>
<mi>&Delta;</mi>
<mi>d</mi>
</mtd>
</mtr>
<mtr>
<mtd>
<mi>&Delta;</mi>
<mi>&theta;</mi>
</mtd>
</mtr>
<mtr>
<mtd>
<mi>&Delta;</mi>
<mi>&beta;</mi>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>;</mo>
</mrow>
Wherein, dnRepresent the deviation of robot end position, Ma、Mα、Md、Mθ、MβIt is that robot kinematics parameter error is
Matrix number, is 3 × n matrix, and Δ a, Δ α, Δ d, Δ θ, Δ β represent the kinematics parameters deviation of robot, wherein Δ θ=(Δ
θ1,Δθ2,...,Δθn), Δ d=(Δ d1,Δd2,...,Δdn), Δ a=(Δ a1,Δa2,...,Δan), Δ α=(Δ
α1,Δα2,...,Δαn), Δ β=(Δ β1,Δβ2,...,Δβn), Δ ai、Δαi、Δdi、Δθi、ΔβiRepresent robot
The little deviation of i-th articular kinesiology parameter, 1≤i≤n, n are the quantity of robot rotary articulation.
3. a kind of robot kinematics' parameter calibration method based on displacement sensor for pull rope according to claim 1, its
It is characterised by, the constraint equation between robot end's position deviation and robot kinematics' parameter error is obtained in step S1,
Comprise the following steps that：
S1.1：According to the MDH models of robot, the kinematic relation obtained between the adjacent two joint of robot is：
<mrow>
<mtable>
<mtr>
<mtd>
<mrow>
<mmultiscripts>
<mi>T</mi>
<mi>i</mi>
<mrow>
<mi>i</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</mmultiscripts>
<mo>=</mo>
<mi>R</mi>
<mi>o</mi>
<mi>t</mi>
<mrow>
<mo>(</mo>
<msub>
<mi>Z</mi>
<mrow>
<mi>i</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<mo>,</mo>
<msub>
<mi>&theta;</mi>
<mi>i</mi>
</msub>
<mo>)</mo>
</mrow>
<mo>&CenterDot;</mo>
<mi>T</mi>
<mi>r</mi>
<mi>a</mi>
<mi>n</mi>
<mi>s</mi>
<mrow>
<mo>(</mo>
<mn>0</mn>
<mo>,</mo>
<mn>0</mn>
<mo>,</mo>
<msub>
<mi>d</mi>
<mi>i</mi>
</msub>
<mo>)</mo>
</mrow>
<mo>&CenterDot;</mo>
<mi>T</mi>
<mi>r</mi>
<mi>a</mi>
<mi>n</mi>
<mi>s</mi>
<mrow>
<mo>(</mo>
<msub>
<mi>a</mi>
<mi>i</mi>
</msub>
<mo>,</mo>
<mn>0</mn>
<mo>,</mo>
<mn>0</mn>
<mo>)</mo>
</mrow>
<mo>&CenterDot;</mo>
<mi>R</mi>
<mi>o</mi>
<mi>t</mi>
<mrow>
<mo>(</mo>
<msub>
<mi>X</mi>
<mi>i</mi>
</msub>
<mo>,</mo>
<msub>
<mi>&alpha;</mi>
<mi>i</mi>
</msub>
<mo>)</mo>
</mrow>
<mo>&CenterDot;</mo>
<mi>R</mi>
<mi>o</mi>
<mi>t</mi>
<mrow>
<mo>(</mo>
<msub>
<mi>Y</mi>
<mi>i</mi>
</msub>
<mo>,</mo>
<msub>
<mi>&beta;</mi>
<mi>i</mi>
</msub>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mrow>
<msub>
<mi>c&theta;</mi>
<mi>i</mi>
</msub>
<msub>
<mi>c&beta;</mi>
<mi>i</mi>
</msub>
<mo>-</mo>
<msub>
<mi>s&theta;</mi>
<mi>i</mi>
</msub>
<msub>
<mi>s&alpha;</mi>
<mi>i</mi>
</msub>
<msub>
<mi>s&beta;</mi>
<mi>i</mi>
</msub>
</mrow>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<msub>
<mi>s&theta;</mi>
<mi>i</mi>
</msub>
<msub>
<mi>c&alpha;</mi>
<mi>i</mi>
</msub>
</mrow>
</mtd>
<mtd>
<mrow>
<msub>
<mi>c&theta;</mi>
<mi>i</mi>
</msub>
<msub>
<mi>s&beta;</mi>
<mi>i</mi>
</msub>
<mo>+</mo>
<msub>
<mi>s&theta;</mi>
<mi>i</mi>
</msub>
<msub>
<mi>s&alpha;</mi>
<mi>i</mi>
</msub>
<msub>
<mi>c&beta;</mi>
<mi>i</mi>
</msub>
</mrow>
</mtd>
<mtd>
<mrow>
<msub>
<mi>a</mi>
<mi>i</mi>
</msub>
<msub>
<mi>c&theta;</mi>
<mi>i</mi>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>s&theta;</mi>
<mi>i</mi>
</msub>
<msub>
<mi>c&beta;</mi>
<mi>i</mi>
</msub>
<mo>+</mo>
<msub>
<mi>c&theta;</mi>
<mi>i</mi>
</msub>
<msub>
<mi>s&alpha;</mi>
<mi>i</mi>
</msub>
<msub>
<mi>s&beta;</mi>
<mi>i</mi>
</msub>
</mrow>
</mtd>
<mtd>
<mrow>
<msub>
<mi>c&theta;</mi>
<mi>i</mi>
</msub>
<msub>
<mi>c&alpha;</mi>
<mi>i</mi>
</msub>
</mrow>
</mtd>
<mtd>
<mrow>
<msub>
<mi>s&theta;</mi>
<mi>i</mi>
</msub>
<msub>
<mi>s&beta;</mi>
<mi>i</mi>
</msub>
<mo>-</mo>
<msub>
<mi>c&theta;</mi>
<mi>i</mi>
</msub>
<msub>
<mi>s&alpha;</mi>
<mi>i</mi>
</msub>
<msub>
<mi>c&beta;</mi>
<mi>i</mi>
</msub>
</mrow>
</mtd>
<mtd>
<mrow>
<msub>
<mi>a</mi>
<mi>i</mi>
</msub>
<msub>
<mi>s&theta;</mi>
<mi>i</mi>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mo>-</mo>
<msub>
<mi>c&alpha;</mi>
<mi>i</mi>
</msub>
<msub>
<mi>s&beta;</mi>
<mi>i</mi>
</msub>
</mrow>
</mtd>
<mtd>
<mrow>
<msub>
<mi>s&alpha;</mi>
<mi>i</mi>
</msub>
</mrow>
</mtd>
<mtd>
<mrow>
<msub>
<mi>c&alpha;</mi>
<mi>i</mi>
</msub>
<msub>
<mi>c&beta;</mi>
<mi>i</mi>
</msub>
</mrow>
</mtd>
<mtd>
<msub>
<mi>d</mi>
<mi>i</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>1</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
</mrow>
</mtd>
</mtr>
</mtable>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>1</mn>
<mo>)</mo>
</mrow>
</mrow>
Wherein, ai、αi、di、θiAnd βiFor the kinematics parameters in the joint of robot i-th, 1≤i≤n, n are robot rotary articulation
Quantity, c θiRepresent cos θi, s θiRepresent sin θi；
S1.2：Obtaining the error matrix between the adjacent two joint of robot to formula (1) both ends progress differential is：
<mrow>
<mi>d</mi>
<mmultiscripts>
<mi>T</mi>
<mi>i</mi>
<mrow>
<mi>i</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</mmultiscripts>
<mo>=</mo>
<mfrac>
<mrow>
<mo>&part;</mo>
<mmultiscripts>
<mi>T</mi>
<mi>i</mi>
<mrow>
<mi>i</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</mmultiscripts>
</mrow>
<mrow>
<mo>&part;</mo>
<msub>
<mi>a</mi>
<mi>i</mi>
</msub>
</mrow>
</mfrac>
<msub>
<mi>&Delta;a</mi>
<mi>i</mi>
</msub>
<mo>+</mo>
<mfrac>
<mrow>
<mo>&part;</mo>
<mmultiscripts>
<mi>T</mi>
<mi>i</mi>
<mrow>
<mi>i</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</mmultiscripts>
</mrow>
<mrow>
<mo>&part;</mo>
<msub>
<mi>&alpha;</mi>
<mi>i</mi>
</msub>
</mrow>
</mfrac>
<msub>
<mi>&Delta;&alpha;</mi>
<mi>i</mi>
</msub>
<mo>+</mo>
<mfrac>
<mrow>
<mo>&part;</mo>
<mmultiscripts>
<mi>T</mi>
<mi>i</mi>
<mrow>
<mi>i</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</mmultiscripts>
</mrow>
<mrow>
<mo>&part;</mo>
<msub>
<mi>d</mi>
<mi>i</mi>
</msub>
</mrow>
</mfrac>
<msub>
<mi>&Delta;d</mi>
<mi>i</mi>
</msub>
<mo>+</mo>
<mfrac>
<mrow>
<mo>&part;</mo>
<mmultiscripts>
<mi>T</mi>
<mi>i</mi>
<mrow>
<mi>i</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</mmultiscripts>
</mrow>
<mrow>
<mo>&part;</mo>
<msub>
<mi>&theta;</mi>
<mi>i</mi>
</msub>
</mrow>
</mfrac>
<msub>
<mi>&Delta;&theta;</mi>
<mi>i</mi>
</msub>
<mo>+</mo>
<mfrac>
<mrow>
<mo>&part;</mo>
<mmultiscripts>
<mi>T</mi>
<mi>i</mi>
<mrow>
<mi>i</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</mmultiscripts>
</mrow>
<mrow>
<mo>&part;</mo>
<msub>
<mi>&beta;</mi>
<mi>i</mi>
</msub>
</mrow>
</mfrac>
<msub>
<mi>&Delta;&beta;</mi>
<mi>i</mi>
</msub>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>2</mn>
<mo>)</mo>
</mrow>
</mrow>
Wherein, Δ ai、Δαi、Δdi、Δθi、ΔβiRepresent the little deviation of robot the i-th articular kinesiology parameter, orderConvolution (1), is obtained：
<mrow>
<msub>
<mi>Q</mi>
<mi>a</mi>
</msub>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mrow>
<msub>
<mi>c&beta;</mi>
<mi>i</mi>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mrow>
<msub>
<mi>s&beta;</mi>
<mi>i</mi>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>3</mn>
<mo>)</mo>
</mrow>
</mrow>
<mrow>
<msub>
<mi>Q</mi>
<mi>&alpha;</mi>
</msub>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<msub>
<mi>s&beta;</mi>
<mi>i</mi>
</msub>
</mrow>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>s&beta;</mi>
<mi>i</mi>
</msub>
</mrow>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<msub>
<mi>c&beta;</mi>
<mi>i</mi>
</msub>
</mrow>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mrow>
<msub>
<mi>c&beta;</mi>
<mi>i</mi>
</msub>
</mrow>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>4</mn>
<mo>)</mo>
</mrow>
</mrow>
<mrow>
<msub>
<mi>Q</mi>
<mi>d</mi>
</msub>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<msub>
<mi>c&alpha;</mi>
<mi>i</mi>
</msub>
<msub>
<mi>s&beta;</mi>
<mi>i</mi>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mrow>
<msub>
<mi>s&alpha;</mi>
<mi>i</mi>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mrow>
<msub>
<mi>c&alpha;</mi>
<mi>i</mi>
</msub>
<msub>
<mi>c&beta;</mi>
<mi>i</mi>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>5</mn>
<mo>)</mo>
</mrow>
</mrow>
<mrow>
<msub>
<mi>Q</mi>
<mi>&theta;</mi>
</msub>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<msub>
<mi>c&alpha;</mi>
<mi>i</mi>
</msub>
<msub>
<mi>c&beta;</mi>
<mi>i</mi>
</msub>
</mrow>
</mtd>
<mtd>
<mrow>
<msub>
<mi>s&alpha;</mi>
<mi>i</mi>
</msub>
</mrow>
</mtd>
<mtd>
<mrow>
<msub>
<mi>a</mi>
<mi>i</mi>
</msub>
<msub>
<mi>s&alpha;</mi>
<mi>i</mi>
</msub>
<msub>
<mi>s&beta;</mi>
<mi>i</mi>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>c&alpha;</mi>
<mi>i</mi>
</msub>
<msub>
<mi>c&beta;</mi>
<mi>i</mi>
</msub>
</mrow>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mrow>
<msub>
<mi>c&alpha;</mi>
<mi>i</mi>
</msub>
<msub>
<mi>s&beta;</mi>
<mi>i</mi>
</msub>
</mrow>
</mtd>
<mtd>
<mrow>
<msub>
<mi>a</mi>
<mi>i</mi>
</msub>
<msub>
<mi>c&alpha;</mi>
<mi>i</mi>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mo>-</mo>
<msub>
<mi>s&alpha;</mi>
<mi>i</mi>
</msub>
</mrow>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<msub>
<mi>c&alpha;</mi>
<mi>i</mi>
</msub>
<msub>
<mi>s&beta;</mi>
<mi>i</mi>
</msub>
</mrow>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<msub>
<mi>a</mi>
<mi>i</mi>
</msub>
<msub>
<mi>s&alpha;</mi>
<mi>i</mi>
</msub>
<msub>
<mi>c&beta;</mi>
<mi>i</mi>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>6</mn>
<mo>)</mo>
</mrow>
</mrow>
<mrow>
<msub>
<mi>Q</mi>
<mi>&beta;</mi>
</msub>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>1</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mo>-</mo>
<mn>1</mn>
</mrow>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>7</mn>
<mo>)</mo>
</mrow>
</mrow>
Formula (2) is transformed to according to formula (3)~(7)：
<mrow>
<mi>d</mi>
<mmultiscripts>
<mi>T</mi>
<mi>i</mi>
<mrow>
<mi>i</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</mmultiscripts>
<mo>=</mo>
<mmultiscripts>
<mi>T</mi>
<mi>i</mi>
<mrow>
<mi>i</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</mmultiscripts>
<mrow>
<mo>(</mo>
<msub>
<mi>Q</mi>
<mi>a</mi>
</msub>
<msub>
<mi>&Delta;a</mi>
<mi>i</mi>
</msub>
<mo>+</mo>
<msub>
<mi>Q</mi>
<mi>&alpha;</mi>
</msub>
<msub>
<mi>&Delta;&alpha;</mi>
<mi>i</mi>
</msub>
<mo>+</mo>
<msub>
<mi>Q</mi>
<mi>d</mi>
</msub>
<msub>
<mi>&Delta;d</mi>
<mi>i</mi>
</msub>
<mo>+</mo>
<msub>
<mi>Q</mi>
<mi>&theta;</mi>
</msub>
<msub>
<mi>&Delta;&theta;</mi>
<mi>i</mi>
</msub>
<mo>+</mo>
<msub>
<mi>Q</mi>
<mi>&beta;</mi>
</msub>
<msub>
<mi>&Delta;&beta;</mi>
<mi>i</mi>
</msub>
<mo>)</mo>
</mrow>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>8</mn>
<mo>)</mo>
</mrow>
</mrow>
It is defined hereinForError matrix, so as to obtain：
<mrow>
<mi>&delta;</mi>
<mmultiscripts>
<mi>T</mi>
<mi>i</mi>
<mrow>
<mi>i</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</mmultiscripts>
<mo>=</mo>
<msub>
<mi>Q</mi>
<mi>a</mi>
</msub>
<msub>
<mi>&Delta;a</mi>
<mi>i</mi>
</msub>
<mo>+</mo>
<msub>
<mi>Q</mi>
<mi>&alpha;</mi>
</msub>
<msub>
<mi>&Delta;&alpha;</mi>
<mi>i</mi>
</msub>
<mo>+</mo>
<msub>
<mi>Q</mi>
<mi>d</mi>
</msub>
<msub>
<mi>&Delta;d</mi>
<mi>i</mi>
</msub>
<mo>+</mo>
<msub>
<mi>Q</mi>
<mi>&theta;</mi>
</msub>
<msub>
<mi>&Delta;&theta;</mi>
<mi>i</mi>
</msub>
<mo>+</mo>
<msub>
<mi>Q</mi>
<mi>&beta;</mi>
</msub>
<msub>
<mi>&Delta;&beta;</mi>
<mi>i</mi>
</msub>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>9</mn>
<mo>)</mo>
</mrow>
</mrow>
Formula (3)~(7) are substituted into formula (9) and obtained：
<mrow>
<mtable>
<mtr>
<mtd>
<mrow>
<mi>&delta;</mi>
<mmultiscripts>
<mi>T</mi>
<mi>i</mi>
<mrow>
<mi>i</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</mmultiscripts>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<msub>
<mi>&delta;z</mi>
<mi>i</mi>
</msub>
</mrow>
</mtd>
<mtd>
<mrow>
<msub>
<mi>&delta;y</mi>
<mi>i</mi>
</msub>
</mrow>
</mtd>
<mtd>
<mrow>
<msub>
<mi>dx</mi>
<mi>i</mi>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>&delta;z</mi>
<mi>i</mi>
</msub>
</mrow>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<msub>
<mi>&delta;x</mi>
<mi>i</mi>
</msub>
</mrow>
</mtd>
<mtd>
<mrow>
<msub>
<mi>dy</mi>
<mi>i</mi>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mo>-</mo>
<msub>
<mi>&delta;y</mi>
<mi>i</mi>
</msub>
</mrow>
</mtd>
<mtd>
<mrow>
<msub>
<mi>&delta;x</mi>
<mi>i</mi>
</msub>
</mrow>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mrow>
<msub>
<mi>dz</mi>
<mi>i</mi>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<msub>
<mi>c&alpha;</mi>
<mi>i</mi>
</msub>
<msub>
<mi>c&beta;</mi>
<mi>i</mi>
</msub>
<msub>
<mi>&Delta;&theta;</mi>
<mi>i</mi>
</msub>
<mo>-</mo>
<msub>
<mi>s&beta;</mi>
<mi>i</mi>
</msub>
<msub>
<mi>&Delta;&alpha;</mi>
<mi>i</mi>
</msub>
</mrow>
</mtd>
<mtd>
<mrow>
<msub>
<mi>s&alpha;</mi>
<mi>i</mi>
</msub>
<msub>
<mi>&Delta;&theta;</mi>
<mi>i</mi>
</msub>
<mo>+</mo>
<msub>
<mi>&Delta;&beta;</mi>
<mi>i</mi>
</msub>
</mrow>
</mtd>
<mtd>
<mrow>
<msub>
<mi>c&beta;</mi>
<mi>i</mi>
</msub>
<msub>
<mi>&Delta;a</mi>
<mi>i</mi>
</msub>
<mo>-</mo>
<msub>
<mi>c&alpha;</mi>
<mi>i</mi>
</msub>
<msub>
<mi>s&beta;</mi>
<mi>i</mi>
</msub>
<msub>
<mi>&Delta;d</mi>
<mi>i</mi>
</msub>
<mo>+</mo>
<msub>
<mi>a</mi>
<mi>i</mi>
</msub>
<msub>
<mi>s&beta;</mi>
<mi>i</mi>
</msub>
<msub>
<mi>s&alpha;</mi>
<mi>i</mi>
</msub>
<msub>
<mi>&Delta;&theta;</mi>
<mi>i</mi>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>c&alpha;</mi>
<mi>i</mi>
</msub>
<msub>
<mi>c&beta;</mi>
<mi>i</mi>
</msub>
<msub>
<mi>&Delta;&theta;</mi>
<mi>i</mi>
</msub>
<mo>+</mo>
<msub>
<mi>s&beta;</mi>
<mi>i</mi>
</msub>
<msub>
<mi>&Delta;&alpha;</mi>
<mi>i</mi>
</msub>
</mrow>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mrow>
<msub>
<mi>c&alpha;</mi>
<mi>i</mi>
</msub>
<msub>
<mi>s&beta;</mi>
<mi>i</mi>
</msub>
<msub>
<mi>&Delta;&theta;</mi>
<mi>i</mi>
</msub>
<mo>-</mo>
<msub>
<mi>c&beta;</mi>
<mi>i</mi>
</msub>
<msub>
<mi>&Delta;&alpha;</mi>
<mi>i</mi>
</msub>
</mrow>
</mtd>
<mtd>
<mrow>
<msub>
<mi>s&alpha;</mi>
<mi>i</mi>
</msub>
<msub>
<mi>&Delta;d</mi>
<mi>i</mi>
</msub>
<mo>+</mo>
<msub>
<mi>a</mi>
<mi>i</mi>
</msub>
<msub>
<mi>c&alpha;</mi>
<mi>i</mi>
</msub>
<msub>
<mi>&Delta;&theta;</mi>
<mi>i</mi>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mo>-</mo>
<msub>
<mi>s&alpha;</mi>
<mi>i</mi>
</msub>
<msub>
<mi>&Delta;&theta;</mi>
<mi>i</mi>
</msub>
<mo>-</mo>
<msub>
<mi>&Delta;&beta;</mi>
<mi>i</mi>
</msub>
</mrow>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<msub>
<mi>c&alpha;</mi>
<mi>i</mi>
</msub>
<msub>
<mi>s&beta;</mi>
<mi>i</mi>
</msub>
<msub>
<mi>&Delta;&theta;</mi>
<mi>i</mi>
</msub>
<mo>+</mo>
<msub>
<mi>c&beta;</mi>
<mi>i</mi>
</msub>
<msub>
<mi>&Delta;&alpha;</mi>
<mi>i</mi>
</msub>
</mrow>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mrow>
<msub>
<mi>s&beta;</mi>
<mi>i</mi>
</msub>
<msub>
<mi>&Delta;a</mi>
<mi>i</mi>
</msub>
<mo>+</mo>
<msub>
<mi>c&alpha;</mi>
<mi>i</mi>
</msub>
<msub>
<mi>c&beta;</mi>
<mi>i</mi>
</msub>
<msub>
<mi>&Delta;d</mi>
<mi>i</mi>
</msub>
<mo>-</mo>
<msub>
<mi>a</mi>
<mi>i</mi>
</msub>
<msub>
<mi>c&beta;</mi>
<mi>i</mi>
</msub>
<msub>
<mi>s&alpha;</mi>
<mi>i</mi>
</msub>
<msub>
<mi>&Delta;&theta;</mi>
<mi>i</mi>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>1</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
</mrow>
</mtd>
</mtr>
</mtable>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>10</mn>
<mo>)</mo>
</mrow>
</mrow>
Obtained according to formula (10)：
<mrow>
<msub>
<mi>d</mi>
<mi>i</mi>
</msub>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mrow>
<msub>
<mi>dx</mi>
<mi>i</mi>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>dy</mi>
<mi>i</mi>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>dz</mi>
<mi>i</mi>
</msub>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mrow>
<msub>
<mi>c&beta;</mi>
<mi>i</mi>
</msub>
<msub>
<mi>&Delta;a</mi>
<mi>i</mi>
</msub>
<mo>-</mo>
<msub>
<mi>c&alpha;</mi>
<mi>i</mi>
</msub>
<msub>
<mi>s&beta;</mi>
<mi>i</mi>
</msub>
<msub>
<mi>&Delta;d</mi>
<mi>i</mi>
</msub>
<mo>+</mo>
<msub>
<mi>a</mi>
<mi>i</mi>
</msub>
<msub>
<mi>s&beta;</mi>
<mi>i</mi>
</msub>
<msub>
<mi>s&alpha;</mi>
<mi>i</mi>
</msub>
<msub>
<mi>&Delta;&theta;</mi>
<mi>i</mi>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>s&alpha;</mi>
<mi>i</mi>
</msub>
<msub>
<mi>&Delta;d</mi>
<mi>i</mi>
</msub>
<mo>+</mo>
<msub>
<mi>a</mi>
<mi>i</mi>
</msub>
<msub>
<mi>c&alpha;</mi>
<mi>i</mi>
</msub>
<msub>
<mi>&Delta;&theta;</mi>
<mi>i</mi>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>s&beta;</mi>
<mi>i</mi>
</msub>
<msub>
<mi>&Delta;a</mi>
<mi>i</mi>
</msub>
<mo>+</mo>
<msub>
<mi>c&alpha;</mi>
<mi>i</mi>
</msub>
<msub>
<mi>c&beta;</mi>
<mi>i</mi>
</msub>
<msub>
<mi>&Delta;d</mi>
<mi>i</mi>
</msub>
<mo>-</mo>
<msub>
<mi>a</mi>
<mi>i</mi>
</msub>
<msub>
<mi>c&beta;</mi>
<mi>i</mi>
</msub>
<msub>
<mi>s&alpha;</mi>
<mi>i</mi>
</msub>
<msub>
<mi>&Delta;&theta;</mi>
<mi>i</mi>
</msub>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mrow>
<msub>
<mi>c&beta;</mi>
<mi>i</mi>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>s&beta;</mi>
<mi>i</mi>
</msub>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>&CenterDot;</mo>
<msub>
<mi>&Delta;a</mi>
<mi>i</mi>
</msub>
<mo>+</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mrow>
<mo>-</mo>
<msub>
<mi>c&alpha;</mi>
<mi>i</mi>
</msub>
<msub>
<mi>s&beta;</mi>
<mi>i</mi>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>s&alpha;</mi>
<mi>i</mi>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>c&alpha;</mi>
<mi>i</mi>
</msub>
<msub>
<mi>c&beta;</mi>
<mi>i</mi>
</msub>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>&CenterDot;</mo>
<msub>
<mi>&Delta;d</mi>
<mi>i</mi>
</msub>
<mo>+</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mrow>
<msub>
<mi>a</mi>
<mi>i</mi>
</msub>
<msub>
<mi>s&beta;</mi>
<mi>i</mi>
</msub>
<msub>
<mi>s&alpha;</mi>
<mi>i</mi>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>a</mi>
<mi>i</mi>
</msub>
<msub>
<mi>c&alpha;</mi>
<mi>i</mi>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mo>-</mo>
<msub>
<mi>a</mi>
<mi>i</mi>
</msub>
<msub>
<mi>c&beta;</mi>
<mi>i</mi>
</msub>
<msub>
<mi>s&alpha;</mi>
<mi>i</mi>
</msub>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>&CenterDot;</mo>
<msub>
<mi>&Delta;&theta;</mi>
<mi>i</mi>
</msub>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>11</mn>
<mo>)</mo>
</mrow>
</mrow>
<mrow>
<msub>
<mi>&delta;</mi>
<mi>i</mi>
</msub>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mrow>
<msub>
<mi>&delta;x</mi>
<mi>i</mi>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>&delta;y</mi>
<mi>i</mi>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>&delta;z</mi>
<mi>i</mi>
</msub>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mrow>
<mo>-</mo>
<msub>
<mi>c&alpha;</mi>
<mi>i</mi>
</msub>
<msub>
<mi>s&beta;</mi>
<mi>i</mi>
</msub>
<msub>
<mi>&Delta;&theta;</mi>
<mi>i</mi>
</msub>
<mo>+</mo>
<msub>
<mi>c&beta;</mi>
<mi>i</mi>
</msub>
<msub>
<mi>&Delta;&alpha;</mi>
<mi>i</mi>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>s&alpha;</mi>
<mi>i</mi>
</msub>
<msub>
<mi>&Delta;&theta;</mi>
<mi>i</mi>
</msub>
<mo>+</mo>
<msub>
<mi>&Delta;&beta;</mi>
<mi>i</mi>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>c&alpha;</mi>
<mi>i</mi>
</msub>
<msub>
<mi>c&beta;</mi>
<mi>i</mi>
</msub>
<msub>
<mi>&Delta;&theta;</mi>
<mi>i</mi>
</msub>
<mo>+</mo>
<msub>
<mi>s&beta;</mi>
<mi>i</mi>
</msub>
<msub>
<mi>&Delta;&alpha;</mi>
<mi>i</mi>
</msub>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mrow>
<msub>
<mi>c&beta;</mi>
<mi>i</mi>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>s&beta;</mi>
<mi>i</mi>
</msub>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>&CenterDot;</mo>
<msub>
<mi>&Delta;&alpha;</mi>
<mi>i</mi>
</msub>
<mo>+</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mrow>
<mo>-</mo>
<msub>
<mi>c&alpha;</mi>
<mi>i</mi>
</msub>
<msub>
<mi>s&beta;</mi>
<mi>i</mi>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>s&alpha;</mi>
<mi>i</mi>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>c&alpha;</mi>
<mi>i</mi>
</msub>
<msub>
<mi>c&beta;</mi>
<mi>i</mi>
</msub>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>&CenterDot;</mo>
<msub>
<mi>&Delta;&theta;</mi>
<mi>i</mi>
</msub>
<mo>+</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>1</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>&CenterDot;</mo>
<msub>
<mi>&Delta;&beta;</mi>
<mi>i</mi>
</msub>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>12</mn>
<mo>)</mo>
</mrow>
</mrow>
Wherein, diAnd δiRepresent respectivelySite error and attitude error；
S1.3：Error matrix between the adjacent two joint of robot is substituted into the transmission chain structure of robot, obtained：
<mrow>
<mmultiscripts>
<mi>T</mi>
<mi>n</mi>
<mn>0</mn>
</mmultiscripts>
<mo>+</mo>
<mi>d</mi>
<mmultiscripts>
<mi>T</mi>
<mi>n</mi>
<mn>0</mn>
</mmultiscripts>
<mo>=</mo>
<mrow>
<mo>(</mo>
<mmultiscripts>
<mi>T</mi>
<mn>1</mn>
<mn>0</mn>
</mmultiscripts>
<mo>+</mo>
<mi>d</mi>
<mmultiscripts>
<mi>T</mi>
<mn>1</mn>
<mn>0</mn>
</mmultiscripts>
<mo>)</mo>
</mrow>
<mrow>
<mo>(</mo>
<mmultiscripts>
<mi>T</mi>
<mn>2</mn>
<mn>1</mn>
</mmultiscripts>
<mo>+</mo>
<mi>d</mi>
<mmultiscripts>
<mi>T</mi>
<mn>2</mn>
<mn>1</mn>
</mmultiscripts>
<mo>)</mo>
</mrow>
<mn>...</mn>
<mrow>
<mo>(</mo>
<mmultiscripts>
<mi>T</mi>
<mi>n</mi>
<mrow>
<mi>n</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</mmultiscripts>
<mo>+</mo>
<mi>d</mi>
<mmultiscripts>
<mi>T</mi>
<mi>n</mi>
<mrow>
<mi>n</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</mmultiscripts>
<mo>)</mo>
</mrow>
<mo>=</mo>
<munderover>
<mo>&Pi;</mo>
<mrow>
<mi>i</mi>
<mo>=</mo>
<mn>1</mn>
</mrow>
<mi>n</mi>
</munderover>
<mrow>
<mo>(</mo>
<mmultiscripts>
<mi>T</mi>
<mi>i</mi>
<mrow>
<mi>i</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</mmultiscripts>
<mo>+</mo>
<mi>d</mi>
<mmultiscripts>
<mi>T</mi>
<mi>i</mi>
<mrow>
<mi>i</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</mmultiscripts>
<mo>)</mo>
</mrow>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>13</mn>
<mo>)</mo>
</mrow>
</mrow>
Wherein,Position orientation relation of i-th joint relative to the i-th -1 joint is represented,Represent that the i-th joint is closed relative to i-th -1
The position and attitude error of section,For the minor variations of robot end's pose,Represent the theoretical position of robot end, definitionUn+1For unit matrix, using Differential Principle, ignore higher differentiation item, obtain：
<mrow>
<mi>d</mi>
<mmultiscripts>
<mi>T</mi>
<mi>n</mi>
<mn>0</mn>
</mmultiscripts>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<mi>&delta;</mi>
<mi>z</mi>
</mrow>
</mtd>
<mtd>
<mrow>
<mi>&delta;</mi>
<mi>y</mi>
</mrow>
</mtd>
<mtd>
<mrow>
<mi>d</mi>
<mi>x</mi>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mi>&delta;</mi>
<mi>z</mi>
</mrow>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<mi>&delta;</mi>
<mi>x</mi>
</mrow>
</mtd>
<mtd>
<mrow>
<mi>d</mi>
<mi>y</mi>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mo>-</mo>
<mi>&delta;</mi>
<mi>y</mi>
</mrow>
</mtd>
<mtd>
<mrow>
<mi>&delta;</mi>
<mi>x</mi>
</mrow>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mrow>
<mi>d</mi>
<mi>z</mi>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>1</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>=</mo>
<munderover>
<mo>&Sigma;</mo>
<mrow>
<mi>i</mi>
<mo>=</mo>
<mn>1</mn>
</mrow>
<mi>n</mi>
</munderover>
<mrow>
<mo>(</mo>
<mmultiscripts>
<mi>T</mi>
<mn>1</mn>
<mn>0</mn>
</mmultiscripts>
<mn>...</mn>
<mmultiscripts>
<mi>T</mi>
<mi>i</mi>
<mrow>
<mi>i</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</mmultiscripts>
<mo>&CenterDot;</mo>
<mi>&delta;</mi>
<mmultiscripts>
<mi>T</mi>
<mi>i</mi>
<mrow>
<mi>i</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</mmultiscripts>
<mo>&CenterDot;</mo>
<mmultiscripts>
<mi>T</mi>
<mrow>
<mi>i</mi>
<mo>+</mo>
<mn>1</mn>
</mrow>
<mi>i</mi>
</mmultiscripts>
<mn>...</mn>
<mmultiscripts>
<mi>T</mi>
<mi>n</mi>
<mrow>
<mi>n</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</mmultiscripts>
<mo>)</mo>
</mrow>
<mo>=</mo>
<mmultiscripts>
<mi>T</mi>
<mi>n</mi>
<mn>0</mn>
</mmultiscripts>
<mo>&CenterDot;</mo>
<munderover>
<mo>&Sigma;</mo>
<mrow>
<mi>i</mi>
<mo>=</mo>
<mn>1</mn>
</mrow>
<mi>n</mi>
</munderover>
<mrow>
<mo>(</mo>
<msubsup>
<mi>U</mi>
<mrow>
<mi>i</mi>
<mo>+</mo>
<mn>1</mn>
</mrow>
<mrow>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msubsup>
<mi>&delta;</mi>
<mmultiscripts>
<mi>T</mi>
<mi>i</mi>
<mrow>
<mi>i</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</mmultiscripts>
<mo>&CenterDot;</mo>
<msub>
<mi>U</mi>
<mrow>
<mi>i</mi>
<mo>+</mo>
<mn>1</mn>
</mrow>
</msub>
<mo>)</mo>
</mrow>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>14</mn>
<mo>)</mo>
</mrow>
</mrow>
Wherein,It is Ui+1Inverse matrix；
Formula (1) and formula (10) are substituted into formula (14), obtain robot end's position deviation and robot kinematics' ginseng
Constraint equation between number deviation：
<mrow>
<msup>
<mi>d</mi>
<mi>n</mi>
</msup>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mrow>
<mi>d</mi>
<mi>x</mi>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mi>d</mi>
<mi>y</mi>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mi>d</mi>
<mi>z</mi>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>=</mo>
<mmultiscripts>
<mi>T</mi>
<mi>n</mi>
<mn>0</mn>
</mmultiscripts>
<mo>&CenterDot;</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mrow>
<mo>&Sigma;</mo>
<mo>&lsqb;</mo>
<msubsup>
<mi>n</mi>
<mrow>
<mi>i</mi>
<mo>+</mo>
<mn>1</mn>
</mrow>
<mi>u</mi>
</msubsup>
<mo>&CenterDot;</mo>
<msub>
<mi>d</mi>
<mi>i</mi>
</msub>
<mo>+</mo>
<mrow>
<mo>(</mo>
<msubsup>
<mi>p</mi>
<mrow>
<mi>i</mi>
<mo>+</mo>
<mn>1</mn>
</mrow>
<mi>u</mi>
</msubsup>
<mo>&times;</mo>
<msubsup>
<mi>n</mi>
<mrow>
<mi>i</mi>
<mo>+</mo>
<mn>1</mn>
</mrow>
<mi>u</mi>
</msubsup>
<mo>)</mo>
</mrow>
<mo>&CenterDot;</mo>
<msub>
<mi>&delta;</mi>
<mi>i</mi>
</msub>
<mo>&rsqb;</mo>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mo>&Sigma;</mo>
<mo>&lsqb;</mo>
<msubsup>
<mi>o</mi>
<mrow>
<mi>i</mi>
<mo>+</mo>
<mn>1</mn>
</mrow>
<mi>u</mi>
</msubsup>
<mo>&CenterDot;</mo>
<msub>
<mi>d</mi>
<mi>i</mi>
</msub>
<mo>+</mo>
<mrow>
<mo>(</mo>
<msubsup>
<mi>p</mi>
<mrow>
<mi>i</mi>
<mo>+</mo>
<mn>1</mn>
</mrow>
<mi>u</mi>
</msubsup>
<mo>&times;</mo>
<msubsup>
<mi>o</mi>
<mrow>
<mi>i</mi>
<mo>+</mo>
<mn>1</mn>
</mrow>
<mi>u</mi>
</msubsup>
<mo>)</mo>
</mrow>
<mo>&CenterDot;</mo>
<msub>
<mi>&delta;</mi>
<mi>i</mi>
</msub>
<mo>&rsqb;</mo>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mo>&Sigma;</mo>
<mo>&lsqb;</mo>
<msubsup>
<mi>a</mi>
<mrow>
<mi>i</mi>
<mo>+</mo>
<mn>1</mn>
</mrow>
<mi>u</mi>
</msubsup>
<mo>&CenterDot;</mo>
<msub>
<mi>d</mi>
<mi>i</mi>
</msub>
<mo>+</mo>
<mrow>
<mo>(</mo>
<msubsup>
<mi>p</mi>
<mrow>
<mi>i</mi>
<mo>+</mo>
<mn>1</mn>
</mrow>
<mi>u</mi>
</msubsup>
<mo>&times;</mo>
<msubsup>
<mi>a</mi>
<mrow>
<mi>i</mi>
<mo>+</mo>
<mn>1</mn>
</mrow>
<mi>u</mi>
</msubsup>
<mo>)</mo>
</mrow>
<mo>&CenterDot;</mo>
<msub>
<mi>&delta;</mi>
<mi>i</mi>
</msub>
<mo>&rsqb;</mo>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>=</mo>
<mo>&lsqb;</mo>
<mtable>
<mtr>
<mtd>
<msub>
<mi>M</mi>
<mi>a</mi>
</msub>
</mtd>
<mtd>
<msub>
<mi>M</mi>
<mi>&alpha;</mi>
</msub>
</mtd>
<mtd>
<msub>
<mi>M</mi>
<mi>d</mi>
</msub>
</mtd>
<mtd>
<msub>
<mi>M</mi>
<mi>&theta;</mi>
</msub>
</mtd>
<mtd>
<msub>
<mi>M</mi>
<mi>&beta;</mi>
</msub>
</mtd>
</mtr>
</mtable>
<mo>&rsqb;</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mi>&Delta;</mi>
<mi>a</mi>
</mtd>
</mtr>
<mtr>
<mtd>
<mi>&Delta;</mi>
<mi>&alpha;</mi>
</mtd>
</mtr>
<mtr>
<mtd>
<mi>&Delta;</mi>
<mi>d</mi>
</mtd>
</mtr>
<mtr>
<mtd>
<mi>&Delta;</mi>
<mi>&theta;</mi>
</mtd>
</mtr>
<mtr>
<mtd>
<mi>&Delta;</mi>
<mi>&beta;</mi>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>15</mn>
<mo>)</mo>
</mrow>
</mrow>
Wherein,WithMatrix U is represented respectivelyi+1The vector of four 3 × 1；dnRepresent robot end position
The deviation put, Ma、Mα、Md、Mθ、MβIt is that robot kinematics' parameter is inclined for the 3 × n matrix obtained according to formula (15) abbreviation
The coefficient matrix of difference, Δ a, Δ α, Δ d, Δ θ, Δ β represent the kinematics parameters deviation of robot, wherein, Δ θ=(Δ θ1,Δ
θ2,...,Δθn), Δ d=(Δ d1,Δd2,...,Δdn), Δ a=(Δ a1,Δa2,...,Δan), Δ α=(Δ α1,Δ
α2,...,Δαn), Δ β=(Δ β1,Δβ2,...,Δβn)。
4. a kind of robot kinematics' parameter calibration method based on displacement sensor for pull rope according to claim 2, its
It is characterised by, Ma、Mα、Md、Mθ、MβIt is relevant with robot each joint angle angle value with the theory movement parameter of robot.
5. a kind of robot kinematics' parameter calibration method based on displacement sensor for pull rope according to claim 1, its
It is characterised by, in the step S2, the range of the displacement sensor for pull rope used is 2000mm, and linear precision 0.05% is heavy
Multiple precision is 0.02%, resolution ratio 0.0244mm, and position measuring system includes at least three displacement sensor for pull rope, installation is drawn
The pedestal of rope displacement transducer and the actuator of carry drawstring.
6. a kind of robot kinematics' parameter calibration method based on displacement sensor for pull rope according to claim 1, its
It is characterised by, in the step S2, determines that the basis coordinates system of robot comprises the following steps：
S2.1：Using the position measuring system being made up of displacement sensor for pull rope determine robot basis coordinates system the origin of coordinates and
Z-direction；
S2.2：The Y direction of robot basis coordinates system is determined using the position measuring system being made up of displacement sensor for pull rope, so
The X-direction of robot is determined using the right-hand rule afterwards.
7. a kind of robot kinematics' parameter calibration method based on displacement sensor for pull rope according to claim 1, its
It is characterised by, in the step S3, according to the robot basis coordinates system determined in step S2, the machine that position measuring system is measured
The position coordinates of device people end is converted to the position coordinates under robot basis coordinates system.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201710865454.9A CN107553493A (en) | 2017-09-22 | 2017-09-22 | A kind of robot kinematics' parameter calibration method based on displacement sensor for pull rope |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201710865454.9A CN107553493A (en) | 2017-09-22 | 2017-09-22 | A kind of robot kinematics' parameter calibration method based on displacement sensor for pull rope |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN107553493A true CN107553493A (en) | 2018-01-09 |
Family
ID=60982589
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201710865454.9A Pending CN107553493A (en) | 2017-09-22 | 2017-09-22 | A kind of robot kinematics' parameter calibration method based on displacement sensor for pull rope |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN107553493A (en) |
Cited By (13)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108705531A (en) * | 2018-04-17 | 2018-10-26 | 上海达野智能科技有限公司 | The kinematic calibration method of industrial robot, calibration system, electronic equipment |
| CN108908335A (en) * | 2018-07-20 | 2018-11-30 | 合肥工业大学 | Robot calibration method based on improved differential evolution algorithm |
| CN109262619A (en) * | 2018-12-20 | 2019-01-25 | 中国铁建重工集团有限公司 | A kind of engineering machinery localization method and system |
| CN109278066A (en) * | 2018-10-30 | 2019-01-29 | 武汉工程大学 | A kind of multirobot basis coordinates system geometric calibration tooling and scaling method |
| CN109655024A (en) * | 2019-01-24 | 2019-04-19 | 大连理工大学 | Using the displacement sensor method for calibrating external parameters of spatial transform technique |
| CN109648606A (en) * | 2018-12-29 | 2019-04-19 | 深圳市越疆科技有限公司 | The bracing wire test method of SCARA robot |
| WO2019148431A1 (en) * | 2018-02-01 | 2019-08-08 | 深圳市大疆创新科技有限公司 | Method and device for calibrating multi-joint mechanism and computer system |
| CN110749257A (en) * | 2019-08-05 | 2020-02-04 | 上海大学 | Measurement datum point non-coplanar error compensation method of pull-wire type measurement system |
| CN110815207A (en) * | 2018-08-14 | 2020-02-21 | 中国科学院沈阳自动化研究所 | Kinematic calibration method for Stewart type parallel flexible cable robot |
| CN111912310A (en) * | 2020-08-10 | 2020-11-10 | 深圳市智流形机器人技术有限公司 | Calibration method, device and equipment |
| CN112077836A (en) * | 2020-09-08 | 2020-12-15 | 北京北特圣迪科技发展有限公司 | Overhead suspender error correction method based on four-flexible-cable traction parallel actuator |
| CN113618738A (en) * | 2021-08-23 | 2021-11-09 | 上海大学 | Mechanical arm kinematic parameter calibration method and system |
| CN114406991A (en) * | 2022-02-21 | 2022-04-29 | 天津工业大学 | Industrial robot calibration and spatial position measurement method based on stay wire type encoder |
Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| FR2332843A1 (en) * | 1975-11-28 | 1977-06-24 | Bendix Corp | CALIBRATION PROCESS OF A SUBSYSTEM OBJECT HANDLING SYSTEM |
| CN101231749A (en) * | 2007-12-20 | 2008-07-30 | 昆山华恒工程技术中心有限公司 | Method for calibrating industry robot |
| CN103231375A (en) * | 2013-04-28 | 2013-08-07 | 苏州大学 | Industrial robot calibration method based on distance error models |
| CN107042527A (en) * | 2017-05-20 | 2017-08-15 | 天津大学 | A kind of industrial robot caliberating device and scaling method based on three-dimensional force sensor |
-
2017
- 2017-09-22 CN CN201710865454.9A patent/CN107553493A/en active Pending
Patent Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| FR2332843A1 (en) * | 1975-11-28 | 1977-06-24 | Bendix Corp | CALIBRATION PROCESS OF A SUBSYSTEM OBJECT HANDLING SYSTEM |
| CN101231749A (en) * | 2007-12-20 | 2008-07-30 | 昆山华恒工程技术中心有限公司 | Method for calibrating industry robot |
| CN103231375A (en) * | 2013-04-28 | 2013-08-07 | 苏州大学 | Industrial robot calibration method based on distance error models |
| CN107042527A (en) * | 2017-05-20 | 2017-08-15 | 天津大学 | A kind of industrial robot caliberating device and scaling method based on three-dimensional force sensor |
Non-Patent Citations (2)
| Title |
|---|
| 薛志奇: "基于线误差的机器人自身参数标定研究", 《中国优秀硕士学位论文全文数据库 信息科技辑》 * |
| 龚星如: "六自由度工业机器人运动学标定的研究", 《中国优秀硕士学位论文全文数据库 信息科技辑》 * |
Cited By (21)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2019148431A1 (en) * | 2018-02-01 | 2019-08-08 | 深圳市大疆创新科技有限公司 | Method and device for calibrating multi-joint mechanism and computer system |
| CN108705531A (en) * | 2018-04-17 | 2018-10-26 | 上海达野智能科技有限公司 | The kinematic calibration method of industrial robot, calibration system, electronic equipment |
| CN108908335A (en) * | 2018-07-20 | 2018-11-30 | 合肥工业大学 | Robot calibration method based on improved differential evolution algorithm |
| CN108908335B (en) * | 2018-07-20 | 2023-03-14 | 合肥工业大学 | Robot calibration method based on improved differential evolution algorithm |
| CN110815207B (en) * | 2018-08-14 | 2022-10-21 | 中国科学院沈阳自动化研究所 | Kinematic calibration method for Stewart type parallel flexible cable robot |
| CN110815207A (en) * | 2018-08-14 | 2020-02-21 | 中国科学院沈阳自动化研究所 | Kinematic calibration method for Stewart type parallel flexible cable robot |
| CN109278066A (en) * | 2018-10-30 | 2019-01-29 | 武汉工程大学 | A kind of multirobot basis coordinates system geometric calibration tooling and scaling method |
| CN109278066B (en) * | 2018-10-30 | 2023-11-03 | 武汉工程大学 | Multi-robot-based coordinate system geometric calibration tool and calibration method |
| CN109262619A (en) * | 2018-12-20 | 2019-01-25 | 中国铁建重工集团有限公司 | A kind of engineering machinery localization method and system |
| CN109648606A (en) * | 2018-12-29 | 2019-04-19 | 深圳市越疆科技有限公司 | The bracing wire test method of SCARA robot |
| CN109655024B (en) * | 2019-01-24 | 2020-05-19 | 大连理工大学 | Method for calibrating external parameters of displacement sensor by adopting space transformation technology |
| CN109655024A (en) * | 2019-01-24 | 2019-04-19 | 大连理工大学 | Using the displacement sensor method for calibrating external parameters of spatial transform technique |
| CN110749257A (en) * | 2019-08-05 | 2020-02-04 | 上海大学 | Measurement datum point non-coplanar error compensation method of pull-wire type measurement system |
| CN110749257B (en) * | 2019-08-05 | 2021-08-10 | 上海大学 | Measurement datum point non-coplanar error compensation method of pull-wire type measurement system |
| CN111912310A (en) * | 2020-08-10 | 2020-11-10 | 深圳市智流形机器人技术有限公司 | Calibration method, device and equipment |
| CN111912310B (en) * | 2020-08-10 | 2021-08-10 | 深圳市智流形机器人技术有限公司 | Calibration method, device and equipment |
| CN112077836A (en) * | 2020-09-08 | 2020-12-15 | 北京北特圣迪科技发展有限公司 | Overhead suspender error correction method based on four-flexible-cable traction parallel actuator |
| CN112077836B (en) * | 2020-09-08 | 2024-01-23 | 北京北特圣迪科技发展有限公司 | Overhead boom error correction method based on four-flexible-cable traction parallel actuator |
| CN113618738A (en) * | 2021-08-23 | 2021-11-09 | 上海大学 | Mechanical arm kinematic parameter calibration method and system |
| CN113618738B (en) * | 2021-08-23 | 2024-04-19 | 上海大学 | Mechanical arm kinematics parameter calibration method and system |
| CN114406991A (en) * | 2022-02-21 | 2022-04-29 | 天津工业大学 | Industrial robot calibration and spatial position measurement method based on stay wire type encoder |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN107553493A (en) | A kind of robot kinematics' parameter calibration method based on displacement sensor for pull rope | |
| CN106737855A (en) | A kind of robot precision compensation method of comprehensive position and attitude error model and rigidity compensation | |
| CN100547614C (en) | A kind of scaling method of industrial robot | |
| CN109773786A (en) | A kind of industrial robot plane precision scaling method | |
| CN110815206B (en) | Kinematics calibration method for Stewart parallel robot | |
| CN107175660B (en) | A kind of six-freedom degree robot kinematics scaling method based on monocular vision | |
| CN104408299B (en) | Robot location's error compensating method based on distance identification redundancy kinematics parameters | |
| CN110281241A (en) | Mechanical arm kinematic calibration method is measured based on laser tracker | |
| CN108789404A (en) | A kind of serial manipulator kinematic calibration method of view-based access control model | |
| CN110202582A (en) | A kind of robot calibration method based on three coordinates platforms | |
| CN108406771A (en) | A kind of plane restriction error model and robot self-calibrating method | |
| CN103231375A (en) | Industrial robot calibration method based on distance error models | |
| CN104535027A (en) | Robot precision compensation method for variable-parameter error recognition | |
| CN110193829A (en) | A kind of robot precision's control method of coupled motions and stiffness parameters identification | |
| CN106338990A (en) | Industrial robot DH parameter calibration and zero position calibration method based on laser tracker | |
| CN108705531A (en) | The kinematic calibration method of industrial robot, calibration system, electronic equipment | |
| CN106777656A (en) | A kind of industrial robot absolute precision calibration method based on PMPSD | |
| CN107144273A (en) | The indoor measurement positioning system base station posture automatic compensating method sensed based on inclination angle | |
| CN104608129A (en) | Planar constraint based robot calibration method | |
| CN104890013A (en) | Pull-cord encoder based calibration method of industrial robot | |
| CN103659806A (en) | Industrial robot zero position defining method | |
| CN102566577A (en) | Method for simply and easily calibrating industrial robot | |
| CN105773622A (en) | Industrial robot absolute accuracy calibrating method based on IEKF | |
| CN107942933A (en) | A kind of grand micro- compound alignment system of the planar three freedom of visual servo and method | |
| CN105643619B (en) | A kind of industrial robot instrument posture control method of use framework description |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| RJ01 | Rejection of invention patent application after publication | ||
| RJ01 | Rejection of invention patent application after publication |
Application publication date: 20180109 |