CN106142083B - The method of the three-dimensional motion emulation of high-altitude curtain wall mounting robot - Google Patents
The method of the three-dimensional motion emulation of high-altitude curtain wall mounting robot Download PDFInfo
- Publication number
- CN106142083B CN106142083B CN201610586252.6A CN201610586252A CN106142083B CN 106142083 B CN106142083 B CN 106142083B CN 201610586252 A CN201610586252 A CN 201610586252A CN 106142083 B CN106142083 B CN 106142083B
- Authority
- CN
- China
- Prior art keywords
- connecting rod
- curtain wall
- wall mounting
- mounting robot
- altitude curtain
- 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.)
- Expired - Fee Related
Links
- 230000033001 locomotion Effects 0.000 title claims abstract description 57
- 238000000034 method Methods 0.000 title claims abstract description 39
- 238000004458 analytical method Methods 0.000 claims abstract description 19
- 230000008859 change Effects 0.000 claims description 22
- 230000006870 function Effects 0.000 claims description 11
- 230000009466 transformation Effects 0.000 claims description 11
- NJPPVKZQTLUDBO-UHFFFAOYSA-N novaluron Chemical compound C1=C(Cl)C(OC(F)(F)C(OC(F)(F)F)F)=CC=C1NC(=O)NC(=O)C1=C(F)C=CC=C1F NJPPVKZQTLUDBO-UHFFFAOYSA-N 0.000 claims description 10
- 238000004088 simulation Methods 0.000 claims description 10
- 230000008569 process Effects 0.000 claims description 8
- 230000009471 action Effects 0.000 claims description 5
- 238000012423 maintenance Methods 0.000 claims 1
- 239000011159 matrix material Substances 0.000 description 20
- 238000010586 diagram Methods 0.000 description 16
- 238000005516 engineering process Methods 0.000 description 7
- 238000009434 installation Methods 0.000 description 7
- 238000010276 construction Methods 0.000 description 4
- 230000003993 interaction Effects 0.000 description 4
- 230000002452 interceptive effect Effects 0.000 description 4
- 230000007246 mechanism Effects 0.000 description 4
- 238000009877 rendering Methods 0.000 description 3
- 239000007787 solid Substances 0.000 description 3
- 230000007704 transition Effects 0.000 description 3
- 238000004422 calculation algorithm Methods 0.000 description 2
- 230000006854 communication Effects 0.000 description 2
- 238000005094 computer simulation Methods 0.000 description 2
- 230000018109 developmental process Effects 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 230000008676 import Effects 0.000 description 2
- 238000000465 moulding Methods 0.000 description 2
- 238000013519 translation Methods 0.000 description 2
- 230000006578 abscission Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 238000004891 communication Methods 0.000 description 1
- 238000011960 computer-aided design Methods 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 230000005672 electromagnetic field Effects 0.000 description 1
- 238000007654 immersion Methods 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 238000011900 installation process Methods 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 239000000463 material Substances 0.000 description 1
- 230000013011 mating Effects 0.000 description 1
- 230000024159 perception of rate of movement Effects 0.000 description 1
- 238000004886 process control Methods 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 230000004044 response Effects 0.000 description 1
- 238000012549 training Methods 0.000 description 1
- 230000000007 visual effect Effects 0.000 description 1
- 230000016776 visual perception Effects 0.000 description 1
- 239000011800 void material Substances 0.000 description 1
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1602—Programme controls characterised by the control system, structure, architecture
- B25J9/1605—Simulation of manipulator lay-out, design, modelling of manipulator
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/10—Programme-controlled manipulators characterised by positioning means for manipulator elements
- B25J9/106—Programme-controlled manipulators characterised by positioning means for manipulator elements with articulated links
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Automation & Control Theory (AREA)
- Manipulator (AREA)
Abstract
The method of the three-dimensional motion emulation of curtain wall mounting robot in high-altitude of the present invention, is related to the assembly equipment of building element, and step is：The essential structure of analysis entities high-altitude curtain wall mounting robot；Three-dimensional modeling is carried out to high-altitude curtain wall mounting robot；Kinematics analysis is carried out to high-altitude curtain wall mounting robot；Assemble virtual robot；Control the virtual robot to move in real time with force feedback equipment, realize high-altitude curtain wall mounting robot three-dimensional motion emulation.The inventive method solves the remote operating delay problem for replacing being accomplished manually the remote control system of curtain wall installment work with high-altitude curtain wall mounting robot in the prior art.
Description
Technical field
Technical scheme is related to the assembly equipment of building element, specifically high-altitude curtain wall mounting robot
The method of three-dimensional motion emulation.
Background technology
Traditional high-altitude curtain wall installation has labor intensity is big, installation effectiveness is low, danger is high, working environment is severe etc.
Shortcoming, in manual installation process, the installation of one piece of curtain wall generally requires several workers and cooperates with completion, and labor intensity is big, accident
Rate is high.Meanwhile building curtain wall installation industry is high-tech industry, technical requirements are of a relatively high, strongly professional, it is necessary to construction enterprises
Workmen of the selection with correlation technique, and skills training is carried out to workmen, this cause manually installed curtain wall cost it is high,
Management difficulty is big.
With the continuous improvement of scientific and technological level, remote control system separates workmen and the installation environment of danger, uses
High-altitude curtain wall mounting robot replaces being accomplished manually curtain wall installment work, and then improves efficiency of construction and installation quality, reduces labor
Fatigue resistance and security risk, overall economic efficiency is improved so as to reduce cost.In the prior art, with high-altitude curtain wall mounting robot
Instead of be accomplished manually curtain wall installment work remote control system operating side and actuating station in communication process, due to the limit of technology
There is certain delay in system, time delay causes the unstable of high-altitude curtain wall mounting robot and is difficult to operate, the behaviour of remote control system
It is the important means for solving remote operating delay problem to make end to use high-altitude curtain wall mounting robot dynamic simulation system.
CN104763160A discloses a kind of high-altitude curtain wall mounting robot, describes the mechanical structure of the robot, but
Its remote control system is not reported, it is necessary to operator operates high-altitude curtain wall mounting robot in actual environment carries out work high above the ground,
With certain danger.
The content of the invention
The technical problems to be solved by the invention are：The side of the three-dimensional motion emulation of high-altitude curtain wall mounting robot is provided
Method, by carrying out three-dimensional modeling to high-altitude curtain wall mounting robot, virtual robot is assembled, this is controlled in real time with force feedback equipment
Virtual robot moves, and realizes high-altitude curtain wall mounting robot three-dimensional motion emulation, solves and use high-altitude curtain wall in the prior art
Mounting robot replaces being accomplished manually the remote operating delay problem of the remote control system of curtain wall installment work.
Technical scheme is used by the present invention solves the technical problem：The three-dimensional motion of high-altitude curtain wall mounting robot is imitated
Genuine method, step are as follows：
The first step, the essential structure of analysis entities high-altitude curtain wall mounting robot：
The essential structure of analysis entities high-altitude curtain wall mounting robot, determine high-altitude curtain wall mounting robot main parts size
Structure snd size；
Second step, three-dimensional modeling is carried out to high-altitude curtain wall mounting robot：
Three-dimensional modeling is carried out to above-mentioned entity high-altitude curtain wall mounting robot using bottom-up mode, according to correlation zero
The structure snd size of part set up the accurate threedimensional model of each parts with solidworks, and save as .STL files；
3rd step, kinematics analysis is carried out to high-altitude curtain wall mounting robot：
Each link rod coordinate system of above-mentioned entity high-altitude curtain wall mounting robot is set, determines the homogeneous coordinate transformation of each connecting rod
Matrix, establish the forward kinematics equation of high-altitude curtain wall mounting robot and inverse kinematics equation and obtain corresponding normal solution and inverse
Solution；
4th step, assemble virtual robot：
(4.1) the accurate threedimensional model of each parts that above-mentioned second step is set up is imported into 3DSMax, establishes line
Coordinate is managed, the articulation nodes for setting each connecting rod of high-altitude curtain wall mounting robot are fulcrum, and export as each parts
.3DS files；
(4.2) virtual operative scenario is established with OpenGL；
(4.3) the .3DS files of each parts of above-mentioned (4.1) step imported into that above-mentioned (4.2) step establishes it is virtual
In operative scenario；
(4.4) assembled one by one according to the assembling between each parts and restriction relation, be assembled into virtual robot,
The original state of high-altitude curtain wall mounting robot is set, high-altitude curtain wall fitting machine is set with the trigonometric function relation of space geometry
The position of each articulation nodes after each link rotatable of device people；
5th step, control the virtual robot to move in real time with force feedback equipment, realize high-altitude curtain wall mounting robot three
Tie up motion simulation：
(5.1) force feedback equipment is configured；
(5.2) force feedback equipment controls the mechanical gripper of high-altitude curtain wall mounting robot to move in real time, the fortune of mechanical gripper
It is dynamic to drive other each link rotatables, realize the three-dimensional motion emulation of high-altitude curtain wall mounting robot.
The method of the three-dimensional motion emulation of above-mentioned high-altitude curtain wall mounting robot, the force feedback equipment is force feedback handle
Novint Falcon, force feedback handle Novint Falcon realize operation by the sensor and data acquisition module of its own
Person's action recognition；The method of the configuration force feedback equipment is installation force feedback handle Novint Falcon software development work
Tool bag sdk, force feedback handle Novint Falcon SDK sdk is configured under C++ environment, the software is opened
Being corresponded to respectively comprising file .include and library file .lib in hair kit sdk is added to comprising in catalogue and library directory,
Complete the configuration of force feedback equipment.
The method of the three-dimensional motion emulation of above-mentioned high-altitude curtain wall mounting robot, the force feedback equipment control high-altitude in real time
The method of the mechanical gripper motion of curtain wall mounting robot is to establish a virtual ball in virtual work scene to replace real midpoint
Change in location, force feedback equipment pass through the sensor of itself, data acquisition module and it is high-frequency refresh operation in real time obtain
Pose coordinate of the force feedback equipment in realistic space, the pose coordinate and force feedback equipment of the virtual ball in virtual scene are set
Pose coordinate in realistic space is mutually matched, and realizes that the movement of force feedback equipment is consistent with the movement of virtual ball, by void
Intend ball to be attached at the distal center of mechanical gripper threedimensional model of importing, mechanical gripper is integrally formed with virtual ball, moves rail
Mark is identical with pose coordinate, and operator controls force feedback equipment motion, and virtual ball moves with force feedback equipment, and programming sets emulation
Clock, the pose of virtual ball, i.e. mechanical gripper end pose are read, carry out the inverse kinematics computing in kinematics analysis, solve
The each joint angle θ of robot1、θ2、θ3、θ4、θ5、θ6And θ7, by above-mentioned each joint angle set each connecting rod articulation nodes position and
Angle change, realize that mechanical gripper drives other each link rotatables, high-frequency refreshing is carried out to this process by simulation clock
Operation, realize that force feedback equipment real time control machine tool handgrip moves, the motion of mechanical gripper drives other each link rotatables, completes
The three-dimensional motion emulation of high-altitude curtain wall mounting robot.
The method of the three-dimensional motion emulation of above-mentioned high-altitude curtain wall mounting robot, " solidworks ", " 3DSMax ",
" stl file ", " 3DS files ", " OpenGL ", " comprising file .include " and " library file .lib " is people in the art
Known to member.
The beneficial effects of the invention are as follows：Compared with prior art, the present invention has substantive distinguishing features following prominent and shown
Write progress：
(1) force feedback refers in interactive process, and input of the computer to operator responds, and anti-by power
Feedback equipment acts on the process of operator.Force feedback interactive system is mainly by operator, force feedback equipment, computer, controlled ring
The mating portions composition such as border and relevant data acquisition, communication.Wherein, controlled environment can be true environment or be meter
The virtual reality of calculation machine analogue simulation.Force feedback equipment realizes operator's action recognition by sensor, data acquisition module, by
Control environment makes respective response according to operator's action, and motion and the interaction force information of the object to be operated are provided to computer；
Computer carries out motion analysis, feedback force calculating and interface display, eventually through force feedback according to the corresponding control algolithm of system
The motor output torque blended space feedback force of equipment acts on operator.Force feedback interactive system is transported by high-frequency refresh
OK, operator is made to perceive continuous power sense and kinesthesia in controlled environment.
(2) virtual reality be computer graphics, computer simulation technique, human-machine interface technology, multimedia technology with
And the cross discipline to grow up on the basis of sensing technology.Existence or non-existence in reality can be simulated and constructed to virtual reality
Environment and object, can assign its corresponding attribute according to demand, including the geometry of object, color, material, texture, hard
Degree, quality etc., gravitational field, electromagnetic field, light and the characteristics of motion in environment etc.；And arbitrarily can add and change, virtually
Operation has greater flexibility and complexity relative to true environment.
Virtual environment be by computer according to user's request by the predetermined Program Generating write, including three-dimensional background,
3D solid etc..Three-dimensional background is used to simulate true environment, background designs true to nature be advantageous to strengthen operator feeling of immersion and
Improve the effect of man-machine interaction；3D solid is used to simulate the object in true environment, is the operation object in virtual environment, with
Operator carries out virtual interacting.Three-dimensional background import can use power Haptic Rendering engine chai3D CWorld classes,
CCamera classes, CLight classes etc. implement, and the attributes such as the visual angle of background, light and pattern can be added and be compiled
Volume.The importing of 3D solid, special Three-dimensional Design Software is generally used in advance, such as solidworks carries out three-dimensional modeling,
Preserved with common STL forms.Due on the emulation platform of chai3D power Haptic Rendering engines, 3DS lattice can be directly read
Stl file form is directly converted to 3DS forms by the file of formula, it is necessary to reuse 3DMAX softwares, finally imported into analogue system
In.
Deepened continuously with the research for putting forth effort haptic interaction technology, its as virtual reality system important component by
It is increasing to pay attention to.This technology has been widely used in computer aided design and manufacture industry, Virtual assemble, distant behaviour
The fields such as work, medical treatment, amusement, education.What the force feedback application requirement based on virtual reality further improved force feedback equipment can
By property, stability and practicality, it is necessary to further investigate the structure and dummy object of virtual environment in terms of virtual reality
Interactive generating algorithm, to obtain simulation effect more true to nature.
(3) thought that the present invention carries out kinematics analysis to robot is：In high-altitude, curtain wall mounting robot joint is built
Vertical rectangular coordinate system, it is then determined that obtained matrix, is then multiplied, most by the transition matrix between each two adjacent coordinates system successively
Mechanical gripper coordinate system is obtained eventually to the matrix of pedestal, so that it is determined that mechanical gripper coordinate system is in the position of base coordinate system and appearance
State.
One connecting rod of high-altitude curtain wall mounting robot can regard a rigid body as.If given the position of certain point on rigid body
Put and the rigid body space posture, then this rigid body be spatially completely it is confirmable.Provided with rigid body a Q, QnTo be firm
Any point on body, { Xn, Yn, ZnIt is a coordinate system being connected with rigid body.Rigid body Q is in basis coordinates system { X0, Y0, Z0In position
It can be expressed as with (4xl) array for homogeneous coordinates form：
P=[x y z 1]T
The posture of rigid body can be represented by the change in coordinate axis direction for the coordinate system that is connected.It is respectively Xn, Yn, Zn coordinate to make n, o, a
The unit direction vector of axle, component of each unit direction vector in basis coordinates system are the direction cosines of connected reference axis, are used
(4xl) array of homogeneous coordinates form is expressed as:
Therefore, the pose of rigid body can use following (4x4) matrix to describe:
Connecting rod n connected coordinate system is arranged on the n of joint；ZnAxle is along joint n axis of movements direction；XnAxle is both perpendicular to Zn
Axle, and perpendicular to Zn+1Axle, its positive direction point to joint n+1 by joint n；YnAxle is determined by the right-hand rule, makes { Xn, Yn, ZnStructure
Into right hand rectangular coordinate system.On origin On, work as Zn-1,ZnWhen intersecting, determined by intersection point；Work as Zn-1,ZnWhen space crossed, by
Their common vertical line and ZnIntersection point determine；Work as Zn-1,ZnWhen parallel, with Zn-1,ZnCommon vertical line and ZnIntersection point as On, and make
The d of next connecting rodn+1=0.
The high-altitude curtain wall mounting robot pedestal origin of coordinates represents that base coordinate system is the coordinate system { X of connecting rod 0 with connecting rod 00,
Y0, Z0, it is fixed, the referential as other link rod coordinate systems, may be simply referred to as basis coordinates system, and make Z0Close on axle edge
Save the direction of principal axis of I 9, O0Be provided with arbitrariness, for convenience's sake, generally make O0With O1Overlap.
The D-H parameters of connecting rod：
an：Connecting rod n length, it is Zn,Zn+1The length of common vertical line；
αn：Connecting rod n torsional angle, Zn,Zn+1Between angle；
θn：Angles of the connecting rod n with respect to connecting rod n-1, referred to as joint angle, are Xn-1,XnBetween angle；
dn：Connecting rod n is relative to connecting rod n-1 skew, joints axes n-1 and n common vertical line and joints axes n and n+1 public affairs
The distance of vertical line.
Angleαn,θnSymbol determined according to the right-hand rule, dnSymbol work as on-1onAlong Zn-1It is just a during axle forward directionnAlways
Positive.
What a coordinate system each connecting rod for high-altitude curtain wall mounting robot establishes, and describes these with homogeneous transformation
Relativeness between coordinate system, also referred to as relative pose.Generally one connecting rod of description relative to relation between adjacent links
Homogeneous transform matrix is designated as the A matrixes of (4x4), represents as follows：
A matrixes are to describe the homogeneous transformation of relative translation and rotation between link rod coordinate system.Al matrixes represent connecting rod I coordinate
It is the homogeneous transformation relative to basis coordinates system, then connecting rod I coordinate system is relative to the pose T1 of basis coordinates system:
T1=T0A1=A1
Wherein T0 is the homogeneous matrix expression formula of basis coordinates system, i.e.,：
A2Matrix represents connecting rod II coordinate system relative to the homogeneous transformation of connecting rod I coordinate system, then connecting rod II coordinate system is in base
Pose T in coordinate system2A can be used2And A1Product represent, and A2Should the right side multiply, i.e.,
T2=A1A2
By that analogy, a n link robots, then have：
Tn=A1A2A3...An-2An-1An
(4) the inventive method establishes threedimensional model in strict accordance with the entity of high-altitude curtain wall mounting robot, and visual perception is strong.
(5) the inventive method analyzes the motion modeling process of high-altitude curtain wall mounting robot, versatile, can use
To the robot of different structure.
(6) the inventive method uses force feedback equipment, and the information in equipment running process controls virtual machine in real time
The motion of people, makes it more tally with the actual situation.
(7) the inventive method establishes high-altitude curtain wall mounting robot and its environment virtual prognostication simulation figure, can make
Operator carries out remote operating under good human-computer interaction interface, effectively solves long time delay to the stability of remote control system and can
The influence of operability.
Brief description of the drawings
The present invention is further described with reference to the accompanying drawings and examples.
Fig. 1 is the step schematic diagram of the method for the three-dimensional motion emulation of curtain wall mounting robot in high-altitude of the present invention；
Fig. 2 is the essential structure schematic diagram of the entity of high-altitude curtain wall mounting robot；
Fig. 3 is the link rod coordinate system schematic diagram of high-altitude curtain wall mounting robot；
Fig. 4 is the coordinate diagram in each joint of original state of virtual high-altitude curtain wall mounting robot；
Fig. 5 is the coordinate diagram that the connecting rod II of virtual high-altitude curtain wall mounting robot rotates；
Fig. 6 is the coordinate diagram that the connecting rod II of virtual high-altitude curtain wall mounting robot and connecting rod III rotate；
Fig. 7 is the coordinate diagram that the connecting rod II, connecting rod III and connecting rod V of virtual high-altitude curtain wall mounting robot rotate；
Fig. 8 is the coordinate diagram that the connecting rod I of virtual high-altitude curtain wall mounting robot rotates；
Fig. 9 is the coordinate diagram that the connecting rod I of virtual high-altitude curtain wall mounting robot and connecting rod V rotate.
In figure, 1. connecting rod I, 2. connecting rod II, 3. connecting rod III, 4. connecting rod IV, 5. connecting rod V, 6. connecting rod VI, 7. connecting rod VII, 8.
Connecting rod VIII, 9. joint I, 10. joint II, 11. joint III, 12. joint IV, 13. joint V, 14. joint VI, 15. joint VII,
16. pedestal.
Embodiment
Embodiment illustrated in fig. 1 shows, the step of the method for the three-dimensional motion emulation of curtain wall mounting robot in high-altitude of the present invention
It is：The essential structure of analysis entities high-altitude curtain wall mounting robot → three-dimensional modeling → right is carried out to high-altitude curtain wall mounting robot
High-altitude curtain wall mounting robot carries out kinematics analysis → assembling virtual robot → with force feedback equipment and controls this virtual in real time
Robot motion, realize high-altitude curtain wall mounting robot three-dimensional motion emulation.
Embodiment illustrated in fig. 2 shows that the major part of the essential structure of high-altitude curtain wall mounting robot entity includes：Pedestal
16th, connecting rod I 1, connecting rod II 2, connecting rod III 3, connecting rod IV 4, connecting rod V 5, connecting rod VI 6, connecting rod VII 7 and connecting rod VIII 8, wherein connecting rod VIII
8 be the mechanical gripper of the high-altitude curtain wall mounting robot；The high-altitude curtain wall mounting robot shares seven joints：Joint I 9, close
Save II 10, joint III 11, joint IV 12, joint V 13, joint VI 14, joint VII 15.Wherein joint VI 14 and the phase of joint VII 15
Overlap on one point, the point can not only swing up and down but also can itself be axle rotary motion, therefore regard two connecting rods as, there are two shutdown
Node and two connected coordinate systems.Designed by high-altitude curtain wall mounting robot body construction, ditetragon mechanism constrains joint
The motion of IV 12, the i.e. joint angles of joint IV 12 are determined by the joint angles of joint II 10 and joint III 11.
Embodiment illustrated in fig. 3 shows that the link rod coordinate system of high-altitude curtain wall mounting robot is to install machine in high-altitude curtain wall
Rectangular coordinate system is established at person joint, its high and medium curtain wall mounting robot pedestal origin of coordinates O0With the origin O of connecting rod I 11Weight
Close, be arranged at the articulation nodes of joint I 9, Z0、Z1Direction of principal axis of the axle along joint I 9, X0、X1Axle is both perpendicular to Z1Axle, and vertically
In Z2Axle, its positive direction point to joint II 10 by joint I 9；The origin O of connecting rod II 22It is arranged at the articulation nodes of joint II 10,
Z2Direction of principal axis of the axle along joint II 10, X2Axle is both perpendicular to Z2Axle, and perpendicular to Z3Axle, its positive direction point to pass by joint II 10
Save III 11；The origin O of connecting rod III 33It is arranged at the articulation nodes of joint III 11, Z3Direction of principal axis of the axle along joint III 11, X3Axle was both
Perpendicular to Z3Axle, and perpendicular to Z4Axle, its positive direction point to joint IV 12 by joint III 11；The origin O of connecting rod IV 44It is arranged on pass
At the articulation nodes for saving IV 12, Z4Direction of principal axis of the axle along joint IV 12, X4Axle is both perpendicular to Z4Axle, and perpendicular to Z5Axle, it is square
Joint V 13 is pointed to by joint IV 12；The origin O of connecting rod V 55It is arranged at the articulation nodes of joint V 13, Z5Axle is along joint
The direction of principal axis of V 13, X5Axle is both perpendicular to Z5Axle, and perpendicular to Z6Axle, its positive direction point to joint VI 14 by joint V 13；Joint
VI 14 and joint VII 15 coincide on one point, the origin O of connecting rod VI 6 and connecting rod VII 76/7It is arranged on joint VI 14 and joint VII 15
Articulation nodes at, Z6Direction of principal axis of the axle along joint VI 14, Z7Direction of principal axis of the axle along joint VII 15, X6Axle is both perpendicular to Z6Axle, again
Perpendicular to Z7Axle, X7Axle is both perpendicular to Z7Axle, and perpendicular to Z8Axle；The origin O of connecting rod VIII 88It is arranged in the end of mechanical gripper
At the heart, Z8Axle and Z7Direction of principal axis is identical, X8Axle is perpendicular to Z8Axle.
Embodiment illustrated in fig. 4 shows, the coordinate in each joint of original state of high-altitude curtain wall mounting robot, wherein L1、L2、
L3、L4、L5、L6/7The respectively joint section of connecting rod I 1, connecting rod II 2, connecting rod III 3, connecting rod IV 4, connecting rod V 5, the VII 7 of connecting rod VI 6/
Point；Articulation nodes L1It is arranged at the origin of world coordinate system, i.e. L1Position coordinates be (0,0,0)；Connecting rod I 1 without occurring
Rotate, then articulation nodes L2Position be (0 ,-a1,0)；Connecting rod II 2 is perpendicular with ground, then the rotational angle θ of connecting rod II 22
For 90, articulation nodes L3Position be (0 ,-a1,a2)；Connecting rod III 3 is parallel with ground, the rotational angle θ of connecting rod III 33For
270, then articulation nodes L4Position be (0 ,-a1-a3,a2)；Connecting rod IV 4 does not rotate, then articulation nodes L5Position be
(0 ,-a1-a3-a4,a2-d5)；Connecting rod V 5 does not rotate, then articulation nodes L6Position be (0 ,-a1-a3-a4-a5,a2-
d5)；Articulation nodes L7With articulation nodes L6Coincide；The rotational angle θ of connecting rod VI 66For 90, connecting rod VII 7 does not rotate；Its
Middle a1、a2、a3、a4、a5, be respectively connecting rod I 1, connecting rod II 2, connecting rod III 3, connecting rod IV 4, the length of connecting rod V 5, d5For connecting rod V 5
Relative to the skew of connecting rod VI 6.
Embodiment illustrated in fig. 5 shows, the coordinate that the connecting rod II of high-altitude curtain wall mounting robot rotates, and wherein connecting rod II 2 is
Moved up and down using x as axle, the rotational angle of connecting rod I 1 is 0, and the rotational angle of connecting rod II 2 is θ2, according to geometric figure
Trigonometric function relation understands that the angle of connecting rod III 3 does not change, articulation nodes L3Position coordinates be changed into
(0,-a1-a2*cos(θ2),a2*sin(θ2))；The angle of connecting rod IV 4 does not change, articulation nodes L4Position
Coordinate is changed into (0 ,-a1-a2*cos(θ2)-a3,a2*sin(θ2)), the angle of connecting rod V 5 does not change, articulation nodes L5Position
Put coordinate and be changed into (0 ,-a1-a2*cos(θ2)-a3-a4,a2*sin(θ2)-d5)；The angle of connecting rod VI 6/7 does not change, joint
Node L6/7Position coordinates be changed into (0 ,-a1-a2*cos(θ2)-a3-a4-a5,a2*sin(θ2)-d5)。
Embodiment illustrated in fig. 6 shows, the coordinate that the connecting rod II and connecting rod III of high-altitude curtain wall mounting robot rotate, wherein connecting
Bar III 3 moves up and down by axle of x, and the rotational angle of connecting rod I 1 is 0, and the rotational angle of connecting rod II 2 is θ2, connecting rod III 3
Rotational angle is θ3, t3=2 π-θ2-θ3, it can be seen from the trigonometric function relation of geometric figure：
The angle of connecting rod IV 4 does not change, articulation nodes L4Position coordinates be：
(0,-a1-a2*cos(θ2)-a3*cos(t3),a2*sin(θ2)-a3*sin(t3))；
The angle of connecting rod V 5 does not change, articulation nodes L5Coordinate be：
(0,-a1-a2*cos(θ2)-a3*cos(t3)-a4,a2*sin(θ2)-a3*sin(t3)-d5)；
The angle of connecting rod VI 6/7 does not change, articulation nodes L6/7Coordinate be：
(0,-a1-a2*cos(θ2)-a3*cos(t3)-a4-a5,a2*sin(θ2)-a3*sin(t3)-d5)。
Embodiment illustrated in fig. 7 shows, the seat that connecting rod II, connecting rod III and the connecting rod V of high-altitude curtain wall mounting robot rotate
Mark, wherein connecting rod V 5 moves forward and backward by axle of z, and the rotational angle of connecting rod I 1 is 0, and the rotational angle of connecting rod II 2 is
θ2, the rotational angle of connecting rod III 3 is θ3, the rotational angle of connecting rod V 5 is θ5, L5Position coordinates be：
(x5,y5,z5),x5=0, y5=-a1-a2*cos(θ2)-a3*cos(t3)-a4,z5=a2*sin(θ2)-a3*sin
(t3)-d5,
It can be seen from the trigonometric function of geometric figure, articulation nodes L6/7Position coordinates be：
(x5+a5sin(θ5),y5-a5cos(θ5),a2*sin(θ2)-a3*sin(t3)-d5),
That is (a5sin(θ5),-a1-a2*cos(θ2)-a3*cos(t3)-a4-a5cos(θ5),a2*sin(θ2)-a3*sin(t3)-
d5)。
Embodiment illustrated in fig. 8 shows that the coordinate of the connecting rod I rotation of high-altitude curtain wall mounting robot, wherein connecting rod I 1 is with z
Moved forward and backward for axle, the angle that connecting rod I 1 rotates is θ1, then connecting rod II 2, connecting rod III 3, connecting rod IV 4, connecting rod V 5 will be with
Z is that axle rotates θ1, and position also will be with changing, and it can be seen from the trigonometric function relation of geometric figure, each joint is saved
Put in the position in x-axis direction, to be changed into by original coordinate 0 | yn|*sin(θ1)；Each articulation nodes in the position in y-axis direction,
Will be by original coordinate yn, it is changed into yn*cos(θ1)；Each articulation nodes are not influenceed, still by connecting rod I 1 in the position in z-axis direction
For zn, i.e., joint II 10 to 13 each articulation nodes of joint V is by former coordinate (0, yn,zn) be changed into (| yn|*sin(θ1),yn*cos
(θ1),zn)。
Embodiment illustrated in fig. 9 shows, the coordinate that the connecting rod I and connecting rod V of high-altitude curtain wall mounting robot rotate, wherein connecting
Bar I 1 and connecting rod V 5 rotate simultaneously, t5=θ1+θ5, L5Position coordinates be (x5,y5,z5),x5=(a1+a2*cos(θ2)+
a3*cos(t3)+a4)*cos(θ1),y5=(- a1-a2*cos(θ2)-a3*cos(t3)-a4*cos(θ1),z5=a2*sin(θ2)-
a3*sin(t3)-d5, it can be seen from the trigonometric function relation of geometric figure, the coordinate of joint VI 14 and joint VII 15 is (x5+
a5sin(t5),y5-a5cos(t5),z5), i.e. ((a1+a2*cos(θ2)+a3*cos(t3)+a4)*cos(θ1)+a5sin(t5), (-
a1-a2*cos(θ2)-a3*cos(t3)-a4*cos(θ1)-a5cos(t5),a2*sin(θ2)-a3*sin(t3)-d5)。
Embodiment
The method of the three-dimensional motion emulation of the high-altitude curtain wall mounting robot of the present embodiment, step are as follows：
The first step, the essential structure of analysis entities high-altitude curtain wall mounting robot：
The essential structure of analysis entities high-altitude curtain wall mounting robot, determine high-altitude curtain wall mounting robot main parts size
Structure snd size；
The essential structure of the entity of the high-altitude curtain wall mounting robot of the present embodiment embodiment as shown in Figure 2 above, the height
Empty curtain wall mounting robot main parts size is that the i.e. link parameters table of structure snd size of each connecting rod is shown in Table 1.
The link parameters table of the high-altitude curtain wall mounting robot of table 1
Listed link parameters are as follows in table 1：
an：Connecting rod n length, it is Zn,Zn+1The length of common vertical line；
αn：Connecting rod n torsional angle, Zn,Zn+1Between angle；
θn：Angles of the connecting rod n with respect to connecting rod n-1, referred to as joint angle, are Xn-1,XnBetween angle；
dn：Connecting rod n is relative to connecting rod n-1 skew, joints axes n-1 and n common vertical line and joints axes n and n+1 public affairs
The distance of vertical line.
Angle [alpha]nAnd θnSymbol determined according to the right-hand rule, dnSymbol work as on-1onAlong Zn-1It is just a during axle forward directionnAlways
It is positive.
Second step, three-dimensional modeling is carried out to high-altitude curtain wall mounting robot：
Three-dimensional modeling is carried out to above-mentioned entity high-altitude curtain wall mounting robot using bottom-up mode, according to correlation zero
The structure snd size of part set up the accurate threedimensional model of each parts with solidworks, and save as .STL files；
According to pedestal 16, connecting rod I, connecting rod II 2, connecting rod III 3, connecting rod IV 4, connecting rod V 5, connecting rod VI 6, connecting rod VII 7 and connect
The Outside Dimensions structure of bar VIII 8 is the length, width and height of parts, establishes the threedimensional model of each parts respectively using solidworks.
The modeling process of Solidworks parts is：Suitable reference plane is chosen first, establishes the plane of each parts
Sketch；Secondly the moulding of the essential characteristic of part is completed using orders such as stretching, mark scanning, rotation, excision, setting-outs；Then
Local moulding is completed using orders such as chamfering, fillets, finally completes the modeling of whole part.Each zero of curtain wall mounting robot
The three-dimensional model diagram of part saves as .STL files after creating.
3rd step, kinematics analysis is carried out to high-altitude curtain wall mounting robot：
Each link rod coordinate system of above-mentioned entity high-altitude curtain wall mounting robot is set, determines the homogeneous coordinate transformation of each connecting rod
Matrix, establish the forward kinematics equation of high-altitude curtain wall mounting robot and inverse kinematics equation and obtain corresponding normal solution and inverse
Solution；
Kinematics analysis is carried out to high-altitude curtain wall mounting robot and typically uses D-H methods, method is, at joint of robot
Rectangular coordinate system is established, it is then determined that obtained matrix, is then multiplied by the transition matrix between each two adjacent coordinates system successively,
Mechanical gripper coordinate system is finally given to the matrix of pedestal, so that it is determined that mechanical gripper coordinate system is in the position of base coordinate system and appearance
State.
One connecting rod of high-altitude curtain wall mounting robot can regard a rigid body as, if the position given certain point on rigid body
Put and the rigid body space posture, then this rigid body be spatially completely it is confirmable.Provided with rigid body a Q, QnTo be firm
Any point on body, { XN,YN,ZnIt is a coordinate system being connected with rigid body, rigid body Q is in basis coordinates system { X0,Y0,Z0In position
It can be expressed as with (4xl) array for homogeneous coordinates form：
P=[x y z 1]T
The posture of rigid body can be represented by the change in coordinate axis direction for the coordinate system that is connected.It is respectively X to make n, o, an、Yn、ZnReference axis
Unit direction vector, component of each unit direction vector in basis coordinates system is the direction cosines of reference axis of being connected, with neat
(4xl) array of secondary coordinate form is expressed as:
Therefore, the pose of rigid body can use following (4x4) matrix to describe:
Connecting rod n connected coordinate system is arranged on the n of joint；ZnAxle is along joint n axis of movements direction；XnAxle is both perpendicular to Zn
Axle, and perpendicular to Zn+1Axle, its positive direction point to joint n+1 by joint n；YnAxle is determined by the right-hand rule, makes { Xn, Yn, ZnStructure
Into right hand rectangular coordinate system.On origin On, work as Zn-1,ZnWhen intersecting, determined by intersection point；Work as Zn-1,ZnWhen space crossed, by
Their common vertical line and ZnIntersection point determine；Work as Zn-1,ZnWhen parallel, with Zn-1,ZnCommon vertical line and ZnIntersection point as On, and make
The d of next connecting rodn+1=0.
The high-altitude curtain wall mounting robot pedestal origin of coordinates represents that base coordinate system is the coordinate system { X of connecting rod 0 with connecting rod 00,
Y0, Z0, it is fixed, the referential as other link rod coordinate systems, may be simply referred to as basis coordinates system, and make Z0Close on axle edge
Save the direction of principal axis of I 9, O0Be provided with arbitrariness, for convenience's sake, generally make O0With O1Overlap.
The D-H parameters of connecting rod are shown in Table 1.
What a coordinate system each connecting rod for high-altitude curtain wall mounting robot establishes, and describes these with homogeneous transformation
Relativeness between coordinate system, also referred to as relative pose.Generally one connecting rod of description relative to relation between adjacent links
Homogeneous transform matrix is designated as the A matrixes of (4x4), represents as follows：
A matrixes are to describe the homogeneous transformation of relative translation and rotation between link rod coordinate system.AlMatrix represents that connecting rod I 1 is sat
Mark system relative to basis coordinates system homogeneous transformation, then the coordinate system of connecting rod I 1 relative to basis coordinates system pose T1For:
T1=T0A1=A1
Wherein T0For the homogeneous matrix expression formula of basis coordinates system, i.e.,：
A2Matrix represents homogeneous transformation of the coordinate system of connecting rod II 2 relative to the coordinate system of connecting rod I 1, then the coordinate system of connecting rod II 2 exists
Pose T in basis coordinates system2A can be used2And A1Product represent, and A2Should the right side multiply, i.e.,
T2=A1A2
By that analogy, a n link robots, then have
Tn=A1A2A3...An-2An-1An
Curtain wall mounting robot is the six-DOF robot with seven rotary joints, special according to the structure of the robot
Point, the coordinate system in each joint is established using D-H methods.{X0, Y0, Z0Base coordinate system is represented, as basis coordinates system；Pedestal coordinate
System and the coordinate system { X of connecting rod I 11, Y1, Z1Overlap；Connecting rod II 2 rotates θ around the y-axis of the coordinate system of connecting rod I 12；Connecting rod III 3 is around connecting rod
The z-axis rotation θ of the coordinate system of II 23；Connecting rod IV 4 rotates θ around the z-axis of the coordinate system of connecting rod III 34；Connecting rod V 5 is around the coordinate system of connecting rod IV 4
Y-axis rotation θ5Translation-d5；Connecting rod VI 6 rotates θ around the y-axis of the coordinate system of connecting rod V 56；Y of the connecting rod VII 7 around the coordinate system of connecting rod VI 6
Axle rotates θ7；{X8,Y8,Z8It is mechanical gripper coordinate system, establish and be in handgrip distal center opening position, the distance of abscission joint VII 15
d8, thereby determine that the link rod coordinate system of curtain wall mounting robot is as shown in Figure 3.
Fig. 3 shows that ditetragon mechanism constrains the motion of joint IV 12, can easily derive joint IV 12
Joint angle：θ4=π-θ2-(θ3- π)=2 π-θ2-θ3.By θ4=2 π-θ2-θ3Bring A into4And by the transition matrix between each coordinate system
Being multiplied can be to obtain from basis coordinates system { X0,Y0,Z0Arrive mechanical gripper coordinate system { X8,Y8,Z8Transition matrix
Wherein：
(here, s23=sin (θ2+θ3), c23=cos (θ2+θ3), sn=sin θn, cn=cos θn)
Above-mentioned formula is Robot kinematics equations, is also forward kinematics solution.So far, curtain wall mounting robot has just been obtained
Each joint angle (θ1、θ2、θ3、θ4、θ5、θ6、θ7) relation between mechanical gripper end position auto―control [n o a p], it is known that
Each joint angle (the θ of robot1、θ2、θ3、θ4、θ5、θ6、θ7), mechanical gripper end position auto―control [n o a p] can be tried to achieve.
Solution in general Inverse Kinematics Solution has three in：Numerical solution, geometric solution and analytic solution.Numerical solution utilizes
Recursive algorithm, fairly simple but precision are poor；Geometric solution is relatively more directly perceived with respect to for other solutions, but its process is complicated, difficult
Spend larger；Analytic method needs to do substantial amounts of matrix operation, but its precision is higher, is particularly suitable for real time computer control use.Examine
Consider the requirement of the feature and real-time of the control of curtain wall mounting robot, the present invention solves Inverse Kinematics using analytic solution
Solution.
If the position auto―control of curtain wall mounting robot hand end is：
According toUnderstandUse respectively
MATLAB solves to its left side L1 and the right R1：
Also according toUnderstandRespectively
Its left side L2 and the right R2 are solved with MATLAB：
Wherein：
1) make (3,3) element of L1=R1 equation the right and lefts equal, i.e.,：-c6=az.Can be in the hope of：
θ6=arccos (aZ)
2) make (3,2) element of L1=R1 equation the right and lefts equal, i.e.,：-s6s7=oz.Can be in the hope of：
3) make (2, the 3) element and (2,4) element difference of L1=R1 equation the right and lefts equal, i.e.,：
Can be in the hope of：
4) make (2,3) element of L1=R1 equation the right and lefts equal, i.e.,：s5s6=-s1aX+c1aY.Can be in the hope of：
5) make (Isosorbide-5-Nitrae) element and (3,4) element difference of L1=R1 equation the right and lefts equal, i.e.,：
Two equatioies of simultaneous can be in the hope of：
(a2c2+a3c23)2+(a2s2+a3s23)2=(c1pX+s1pY-a1-a4-a5c5-d8c5s6)2+(pZ-d5+d8c6)2
Abbreviation obtains：
And then can be in the hope of：
6) make (Isosorbide-5-Nitrae) element of L2=R2 equation the right and lefts equal, i.e.,：
a2+a3c3+a4c2+a5c2c5+d5s2+d8(c2c5s6-s2c6)=pXc1c2+pYs1c2+pZs2-a1c2
It can release：
c2(-pXc1-pYs1+a1+a4+a5c5+d8c5s6)+s2(d5-d8c6-pZ)=- a7-a3c3
Order：M=-pXc1-pYs1+a1+a4+a5c5+d8c5s6, N=d5-d8c6-pZ, R=-a7-a3c3
IfWhereinThenBringing above formula into can obtain
I.e.ThenAnd then it can askThus can be in the hope of：
7) the ditetragon mechanism in intelligent high-altitude curtain wall mounting robot constrains the motion of joint IV 12, can hold very much
The easy joint angle for deriving joint IV 12, i.e.,：
θ4=π-θ2-(θ3- π)=2 π-θ2-θ3
So, the Inverse Kinematics Solution of intelligent high-altitude curtain wall mounting robot just solves each joint variable, it is known that machine
Tool handgrip end position auto―control [n o a p], can try to achieve each joint angle (θ of robot1、θ2、θ3、θ4、θ5、θ6、θ7)。
4th step, assemble virtual robot
(4.1) the accurate threedimensional model of each parts that above-mentioned second step is set up is imported into 3DSMax, establishes line
Coordinate is managed, the articulation nodes for setting each connecting rod of high-altitude curtain wall mounting robot are fulcrum, and export as each parts
.3DS files；Specific method is as follows：
3DSMax is opened, clicks on and imports in the drop-down menu of file, pop-up dialogue box, selects the part to be imported three-dimensional
Illustraton of model .STL files, imported into 3DSMax, establish the texture coordinate of each parts.After importing connecting rod, each connecting rod is set
Articulation nodes are fulcrum, and connecting rod is moved and rotated, and make the origin of 3DSMax world coordinate systems and the pass of connecting rod
Point overlaps successively.Wherein mechanical gripper distal center point will coincide with the origin of world coordinate system, to set mechanical gripper
Position auto―control.Export, pop-up dialogue box, by the threedimensional model of each parts with .3DS texts are clicked in the drop-down menu of file
Part form exports, and completes threedimensional model and saves as 3DS forms and keep texture coordinate.
(4.2) virtual operative scenario is established with OpenGL, it is specific as follows：
Built using the CWorld classes in OpenGL and power Haptic Rendering engine chai3D, CCamera classes, CLight classes etc.
Vertical virtual world, and camera and light source are loaded, as the observation station of operator, level ground is created in bottom, is led to carry
Curtain wall mounting robot after entering, complete the foundation of virtual work scene.
(4.3) the .3DS files of each parts of above-mentioned (4.1) step are imported into the virtual work that above-mentioned (4.2) step establishes
Make in scene, it is specific as follows：
The three-dimensional model information of each parts .3DS files is read with OpenGL codings, in the virtual work scene established
Middle each parts of display, each parts after importing are at the world coordinate system origin all in virtual work scene.
(4.4) assembled one by one according to the assembling between each parts and restriction relation, be assembled into virtual robot,
The original state of high-altitude curtain wall mounting robot is set, high-altitude curtain wall fitting machine is set with the trigonometric function relation of space geometry
The position of each articulation nodes, specific as follows after each link rotatable of device people：
It can be seen from the construction of robot, the movement and rotation of subordinate's connecting rod can all have an impact to all higher level's connecting rods,
As connecting rod I 1 rotates θ1, connecting rod II 2, connecting rod III 3, connecting rod IV 4, connecting rod V 5, connecting rod VI 6, connecting rod VII 7 will rotate θ1.Connecting rod
N-1 end overlaps with connecting rod n articulation nodes, and connecting rod n-1 end determines the position of connecting rod n articulation nodes.
According to the link rod coordinate system schematic diagram and table 1 of above-mentioned Fig. 3 high-altitude curtain wall mounting robot, curtain wall is set to install
The original state of robot, as shown in the coordinate diagram in each joint of original state of the virtual high-altitude curtain wall mounting robots of Fig. 4, wherein
{ X, Y, Z } is the world coordinate system of virtual world.Articulation nodes L1Position be (0,0,0), the rotational angle θ of connecting rod I 11For 0,
Then articulation nodes L2Position be (0 ,-a1,0)；Connecting rod II 2 is perpendicular with ground, then the rotational angle θ of connecting rod II 22For 90,
Articulation nodes L3Position be (0 ,-a1,a2)；Connecting rod III 3 is parallel with ground, the rotational angle θ of connecting rod III 33For 270, then close
Point L successively4Position be (0 ,-a1-a3,a2)；The rotational angle θ of connecting rod IV 44For 0, then articulation nodes L5Position be (0 ,-a1-
a3-a4,a2-d5)；The rotational angle θ of connecting rod V 55For 0, then articulation nodes L6Position be (0 ,-a1-a3-a4-a5,a2-d5)；Joint
Node L7With articulation nodes L6Coincide；The rotational angle θ of connecting rod VI 66For 90, the rotational angle θ of connecting rod VII 77For 0；Wherein a1、a2、
a3、a4、a5, be respectively connecting rod I 1, connecting rod II 2, connecting rod III 3, connecting rod IV 4, the length of connecting rod V 5.By θ1=0, θ2=90, θ3=
270, θ4=0, θ5=0, θ6=90, θ7=0 is brought into forward kinematics equation, can solve the end pose square of mechanical gripper
Battle array [n o a p], thus the pose of mechanical gripper is set, the original state for completing robot is set.
Connecting rod II 2 moves up and down by axle of x, if the rotational angle of connecting rod I 1 is 0, the rotational angle of connecting rod II 2
For θ2, as shown in the coordinate diagram of the connecting rod II rotation of the virtual high-altitude curtain wall mounting robots of Fig. 5, according to the triangle letter of geometric figure
Number relation understands that the angle of connecting rod III 3 does not change, articulation nodes L3Position by former coordinate (0 ,-a1,a2) be changed into (0 ,-
a1-a2*cos(θ2),a2*sin(θ2))；The angle of connecting rod IV 4 does not change, articulation nodes L4Position by former coordinate (0 ,-
a1-a3,a2) it is changed into (0 ,-a1-a2*cos(θ2)-a3,a2*sin(θ2)), the angle of connecting rod V 5 does not change, articulation nodes L5
Position by former coordinate (0 ,-a1-a3-a4,a2-d5) be changed into：
(0,-a1-a2*cos(θ2)-a3-a4,a2*sin(θ2)-d5)；
The angle of connecting rod VI 6/7 does not change, articulation nodes L6/7Position by former coordinate (0 ,-a1-a3-a4-a5,a2-
d5) it is changed into (0 ,-a1-a2*cos(θ2)-a3-a4-a5,a2*sin(θ2)-d5)。
Connecting rod III 3 moves up and down by axle of x, if the rotational angle of connecting rod I 1 is 0, the rotational angle of connecting rod II 2
For θ2, the rotational angle of connecting rod III 3 is θ3, such as the coordinate of connecting rod II and connecting rod the III rotation of the virtual high-altitude curtain wall mounting robots of Fig. 6
Shown in figure, it can be seen from the trigonometric function of geometric figure, t3=2 π-θ2-θ3, the angle of connecting rod IV 4 do not change, joint section
Point L4Coordinate (0 ,-a1-a2*cos(θ2)-a3*cos(t3),a2*sin(θ2)-a3*sin(t3))；The angle of connecting rod V 5 is not sent out
Changing, articulation nodes L5Coordinate be：
(0,-a1-a2*cos(θ2)-a3*cos(t3)-a4,a2*sin(θ2)-a3*sin(t3)-d5)；
The angle of connecting rod VI 6/7 does not change, articulation nodes L6/7Coordinate be：
(0,-a1-a2*cos(θ2)-a3*cos(t3)-a4-a5,a2*sin(θ2)-a3*sin(t3)-d5)。
Wherein ditetragon mechanism constrains the motion of joint IV 12, θ4By θ2And θ3Determine, without considering θ4To other companies
The influence of bar.
Connecting rod V 5 moves forward and backward by axle of z, if the rotational angle of connecting rod I 1 is 0, the rotational angle of connecting rod II 2
For θ2, the rotational angle of connecting rod III 3 is θ3, the rotational angle of connecting rod V 5 is θ5, such as the connecting rod of the virtual high-altitude curtain wall mounting robots of Fig. 7
Shown in the coordinate diagram that II, connecting rod III and connecting rod V rotate, it can be seen from the trigonometric function of geometric figure, articulation nodes L6/7Seat
It is designated as：
(x5+a5sin(θ5),y5-a5cos(θ5),a2*sin(θ2)-a3*sin(t3)-d5),
That is (a5sin(θ5),-a1-a2*cos(θ2)-a3*cos(t3)-a4-a5cos(θ5),a2*sin(θ2)-a3*sin(t3)-
d5)。
Connecting rod I 1 moves forward and backward by axle of z, if the angle that connecting rod I 1 rotates is θ1, then connecting rod II 2, connecting rod III 3,
Connecting rod IV 4, connecting rod V 5 will rotate θ by axle of z1, and position also will be with changing.Such as the virtual high-altitude curtain wall peaces of Fig. 8
Shown in the coordinate diagram that the connecting rod I of machine people rotates, it can be seen from the trigonometric function relation of geometric figure, each articulation nodes are in x
The position of direction of principal axis, to be changed into by original coordinate 0 | yn|*sin(θ1)；Each articulation nodes, be by original in the position in y-axis direction
The coordinate y comen, it is changed into yn*cos(θ1)；Each articulation nodes are not influenceed by connecting rod I 1, are still z in the position in z-axis directionn, i.e.,
Joint II 10 is to 13 each articulation nodes of joint V by former coordinate (0, yn,zn) be changed into (| yn|*sin(θ1),yn*cos(θ1),zn)。
If connecting rod I 1 and connecting rod V 5 rotate simultaneously, connecting rod I and company such as the virtual high-altitude curtain wall mounting robots of Fig. 9
Shown in the coordinate diagram that bar V rotates, t5=θ1+θ5, the coordinate of joint VI 14 and joint VII 15 is：
(x5+a5sin(t5),y5-a5cos(t5),a2*sin(θ2)-a3*sin(t3)-d5),
That is ((a1+a2*cos(θ2)+a3*cos(t3)+a4)*cos(θ1)+a5sin(t5),
(-a1-a2*cos(θ2)-a3*cos(t3)-a4*cos(θ1)-a5cos(t5),a2*sin(θ2)-a3*sin(t3)-d5)。
So known θ1、θ2、θ3、θ4、θ5, wherein t3=2 π-θ2-θ3, t5=θ1+θ5, it may be determined that the joint section in each joint
Point coordinates, i.e.,：
L1(0,0,0),
L2(a1*sin(θ1),-a1*cos(θ1), 0),
L3((a1+a2*cos(θ2))*sin(θ1),(-a1-a2*cos(θ2))*cos(θ1),a2*sin(θ2)),
L4((a1+a2*cos(θ2)+a3*cos(t3))*sin(θ1),(-a1-a2*cos(θ2)-a3*cos(t3))*cos
(θ1),a2*sin(θ2)-a3*sin(t3)),
L5((a1+a2*cos(θ2)+a3*cos(t3)+a4)*cos(θ1),(-a1-a2*cos(θ2)-a3*cos(t3)-a4)*
cos(θ1),a2*sin(θ2)-a3*sin(t3)-d5),
L6/7((a1+a2*cos(θ2)+a3*cos(t3)+a4)*cos(θ1)+a5sin(t5),(-a1-a2*cos(θ2)-a3*
cos(t3)-a4)*cos(θ1)-a5cos(t5),a2*sin(θ2)-a3*sin(t3)-d5)。
5th step, control the virtual robot to move in real time with force feedback equipment, realize high-altitude curtain wall mounting robot three
Tie up motion simulation：
(5.1) force feedback equipment is configured, it is specific as follows：
The force feedback equipment that the present invention uses is force feedback handle Novint Falcon, force feedback handle Novint
Falcon realizes operator's action recognition by sensor, the data acquisition module of itself.Force feedback handle Novint is installed
Falcon SDK sdk, force feedback handle Novint Falcon are configured under C++ environment, by software development work
Being corresponded to respectively comprising file .include and library file .lib in tool bag sdk is added to comprising in catalogue and library directory, completes
The configuration of force feedback equipment.
(5.2) force feedback equipment controls the mechanical gripper of high-altitude curtain wall mounting robot to move in real time, the fortune of mechanical gripper
It is dynamic to drive other each link rotatables, the three-dimensional motion emulation of high-altitude curtain wall mounting robot is realized, it is specific as follows：
For three-dimensional reconstruction into virtual prototype, being capable of control machine people fortune in virtual work scene for curtain wall mounting robot
It is dynamic, in addition to having three-dimensional input and force feedback equipment, it is necessary to a virtual ball is established in virtual work scene and is replaced in reality
The change in location of point.Force feedback equipment can be real by the sensor of itself, data acquisition module and high-frequency refreshing operation
When obtain pose coordinate of the force feedback equipment in realistic space, set the pose coordinate of virtual ball in virtual scene and power anti-
Present pose coordinate of the equipment in realistic space to be mutually matched, realize the movement of force feedback equipment and the mobile phase one of virtual ball
Cause.Virtual ball is attached at the distal center of mechanical gripper threedimensional model of importing, mechanical gripper is integrally formed with virtual ball,
Movement locus is identical with pose coordinate.Operator controls force feedback equipment motion, and virtual ball is moved with force feedback equipment, and programming is set
Simulation clock is put, reads the pose of virtual ball, i.e. mechanical gripper end pose [n o a p], inverse kinematics computing is carried out, solves
Each joint angle (the θ of robot1、θ2、θ3、θ4、θ5、θ6、θ7), set the articulation nodes position of each connecting rod and angle to become by joint angle
Change, realize that mechanical gripper drives other each link rotatables, carry out high-frequency refresh to this process by simulation clock and run, it is real
Existing force feedback equipment real time control machine tool handgrip motion, the motion of mechanical gripper drive other each link rotatables, complete curtain wall peace
The three-dimensional motion emulation of machine people.
The rotation of each connecting rod is specific as follows：
The articulation nodes L of connecting rod I 11For：
(0,0,0) and rotate θ by axle of z1；
The articulation nodes L of connecting rod II 22For：
(a1*sin(θ1),-a1*cos(θ1), 0), and rotate θ by axle of x2, using z as axle rotate θ1；
The articulation nodes L of connecting rod III 33For：
((a1+a2*cos(θ2))*sin(θ1),(-a1-a2*cos(θ2))*cos(θ1),a2*sin(θ2)), and using x as axle
Rotate θ3, using z as axle rotate θ1；
The articulation nodes L of connecting rod IV 44For：
((a1+a2*cos(θ2)+a3*cos(t3))*sin(θ1),(-a1-a2*cos(θ2)-a3*cos(t3))*cos(θ1),
a2*sin(θ2)-a3*sin(t3)), and rotate θ by axle of x4, using z as axle rotate θ1；
The articulation nodes L of connecting rod V 55For：
((a1+a2*cos(θ2)+a3*cos(t3)+a4)*cos(θ1),(-a1-a2*cos(θ2)-a3*cos(t3)-a4)*cos
(θ1),a2*sin(θ2)-a3*sin(t3)-d5), and rotate θ by axle of z1+θ5；
The articulation nodes L of connecting rod VI 6/76/7For：
((a1+a2*cos(θ2)+a3*cos(t3)+a4)*cos(θ1)+a5sin(t5),(-a1-a2*cos(θ2)-a3*cos
(t3)-a4)*cos(θ1)-a5cos(t5),a2*sin(θ2)-a3*sin(t3)-d5), and rotate θ by axle of x6, using y as axle rotate
θ7, using z as axle rotate θ1+θ5。
The method of the three-dimensional motion emulation of above-mentioned high-altitude curtain wall mounting robot, " solidworks ", " 3DSMax ",
" stl file ", " 3DS files ", " OpenGL ", " comprising file .include " and " library file .lib " is people in the art
Known to member.
Claims (3)
1. the method for the three-dimensional motion emulation of high-altitude curtain wall mounting robot, it is characterised in that step is as follows：
The first step, the essential structure of analysis entities high-altitude curtain wall mounting robot：
The essential structure of analysis entities high-altitude curtain wall mounting robot, determine high-altitude curtain wall mounting robot main parts size bag
Include：Pedestal, connecting rod I, connecting rod II, connecting rod III, connecting rod IV, connecting rod V, connecting rod VI, the structure snd size of connecting rod VII and connecting rod VIII；
Second step, three-dimensional modeling is carried out to high-altitude curtain wall mounting robot：
Three-dimensional modeling is carried out to above-mentioned entity high-altitude curtain wall mounting robot using bottom-up mode, according to related components
Structure snd size the threedimensional model of each parts is set up with solidworks, and save as .STL files；
3rd step, kinematics analysis is carried out to high-altitude curtain wall mounting robot：
Each link rod coordinate system of above-mentioned entity high-altitude curtain wall mounting robot is set, determines the homogeneous coordinate transformation square of each connecting rod
Battle array, establish the forward kinematics equation of high-altitude curtain wall mounting robot and inverse kinematics equation and obtain corresponding normal solution and inverse solution；
4th step, assemble virtual robot：
(4.1) threedimensional model for each parts that above-mentioned second step is set up is imported into 3DSMax, establishes texture coordinate, if
The articulation nodes i.e. fulcrum of each connecting rod of high-altitude curtain wall mounting robot is put, and exports as the .3DS texts of each parts
Part；
(4.2) virtual operative scenario is established with OpenGL；
(4.3) the .3DS files of each parts of above-mentioned (4.1) step are imported into the virtual work that above-mentioned (4.2) step establishes
In scene；
(4.4) assembled one by one according to the assembling between each parts and restriction relation, be assembled into virtual robot, set
The original state of high-altitude curtain wall mounting robot, high-altitude curtain wall mounting robot is set with the trigonometric function relation of space geometry
Each link rotatable after each articulation nodes position；
5th step, control the virtual robot to move in real time with force feedback equipment, realize the maintenance and operation of high-altitude curtain wall mounting robot three
Dynamic emulation：
(5.1) force feedback equipment is configured；
(5.2) force feedback equipment controls the mechanical gripper of high-altitude curtain wall mounting robot to move in real time, the motion band of mechanical gripper
Other each link rotatables are moved, realize the three-dimensional motion emulation of high-altitude curtain wall mounting robot.
2. the method for the three-dimensional motion emulation of high-altitude curtain wall mounting robot according to claim 1, it is characterised in that：It is described
Force feedback equipment is force feedback handle Novint Falcon, the sensor that force feedback handle Novint Falcon pass through its own
Operator's action recognition is realized with data acquisition module；The method of the configuration force feedback equipment is to install force feedback handle
Novint Falcon SDK sdk, force feedback handle Novint Falcon software is configured under C++ environment
Development kit sdk, add being corresponded to respectively comprising file .include and library file .lib in SDK sdk
It is added to comprising in catalogue and library directory, completes the configuration of force feedback equipment.
3. the method for the three-dimensional motion emulation of high-altitude curtain wall mounting robot according to claim 1, it is characterised in that：It is described
The method that force feedback equipment controls the mechanical gripper of high-altitude curtain wall mounting robot to move in real time is established in virtual work scene
One virtual ball replaces the change in location at real midpoint, the sensor, data acquisition module and height that force feedback equipment passes through itself
The refreshing operation of frequency obtains pose coordinate of the force feedback equipment in realistic space in real time, sets the virtual ball in virtual scene
Pose coordinate in realistic space of pose coordinate and force feedback equipment be mutually matched, realize the movement of force feedback equipment with it is empty
Intend ball movement it is consistent, virtual ball is attached at the distal center of mechanical gripper threedimensional model of importing, mechanical gripper with
Virtual ball is integrally formed, and movement locus is identical with pose coordinate, and operator controls force feedback equipment motion, and virtual ball is with force feedback
Equipment moves, and programming sets simulation clock, reads the pose of virtual ball, i.e. mechanical gripper end pose, carries out kinematics analysis
In inverse kinematics computing, solve each joint angle θ of robot1、θ2、θ3、θ4、θ5、θ6And θ7, set by above-mentioned each joint angle each
The articulation nodes position of connecting rod and angle change, realize that mechanical gripper drives other each link rotatables, by simulation clock to this
Process carries out high-frequency refresh and run, and realizes that force feedback equipment real time control machine tool handgrip moves, the motion band of mechanical gripper
Other each link rotatables are moved, complete the three-dimensional motion emulation of high-altitude curtain wall mounting robot.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201610586252.6A CN106142083B (en) | 2016-07-21 | 2016-07-21 | The method of the three-dimensional motion emulation of high-altitude curtain wall mounting robot |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201610586252.6A CN106142083B (en) | 2016-07-21 | 2016-07-21 | The method of the three-dimensional motion emulation of high-altitude curtain wall mounting robot |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN106142083A CN106142083A (en) | 2016-11-23 |
| CN106142083B true CN106142083B (en) | 2018-03-16 |
Family
ID=58059734
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201610586252.6A Expired - Fee Related CN106142083B (en) | 2016-07-21 | 2016-07-21 | The method of the three-dimensional motion emulation of high-altitude curtain wall mounting robot |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN106142083B (en) |
Families Citing this family (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CA3078578A1 (en) | 2017-10-06 | 2019-04-11 | Moog Inc. | Teleoperation system, method, apparatus, and computer-readable medium |
| CN108262742A (en) * | 2018-01-24 | 2018-07-10 | 深圳市智能机器人研究院 | The robot and its control method of a kind of modular construction |
| CN109033530A (en) * | 2018-06-28 | 2018-12-18 | 北京道亨时代科技有限公司 | A kind of construction method of turning joint fitting model |
| CN109732593B (en) * | 2018-12-28 | 2021-04-23 | 深圳市越疆科技有限公司 | Remote control method and device for robot and terminal equipment |
| CN109696915B (en) * | 2019-01-07 | 2022-02-08 | 上海托华机器人有限公司 | Test method and system |
| CN109760059B (en) * | 2019-02-28 | 2020-12-08 | 华中科技大学 | Mechanical arm assembly task planning method based on BIM and building assembly method |
| CN112157653B (en) * | 2020-09-11 | 2022-02-01 | 北京如影智能科技有限公司 | Shielding detection method and device |
| CN112541967B (en) * | 2020-12-10 | 2024-09-27 | 杭州和利时自动化有限公司 | Cabinet control simulation method, device, equipment and computer readable storage medium |
| CN114460904B (en) * | 2022-01-25 | 2024-02-02 | 燕山大学 | Digital twin system for gantry robot |
| TWI828369B (en) * | 2022-10-14 | 2024-01-01 | 創博股份有限公司 | Method of automatically establishing a robot model |
Family Cites Families (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JPS60195611A (en) * | 1984-03-16 | 1985-10-04 | Hitachi Ltd | Robot system corresponding to work movement |
| CN103778301A (en) * | 2014-02-21 | 2014-05-07 | 重庆邮电大学 | Mechanical arm simulation method based on virtual prototype technology |
| CN103870665B (en) * | 2014-04-08 | 2017-06-06 | 哈尔滨工业大学 | Space manipulator aids in docking operation three dimension dynamic simulation method |
| CN104763160B (en) * | 2015-03-27 | 2016-08-24 | 河北工业大学 | A kind of high-altitude curtain wall mounting robot |
| CN105302959B (en) * | 2015-10-30 | 2019-03-05 | 福建长江工业有限公司 | A kind of six axis grinding and polishing industrial robot off-line programing methods |
-
2016
- 2016-07-21 CN CN201610586252.6A patent/CN106142083B/en not_active Expired - Fee Related
Also Published As
| Publication number | Publication date |
|---|---|
| CN106142083A (en) | 2016-11-23 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN106142083B (en) | The method of the three-dimensional motion emulation of high-altitude curtain wall mounting robot | |
| Igarashi et al. | Spatial keyframing for performance-driven animation | |
| US11144112B2 (en) | Systems and methods for creating haptic proxies for use in virtual reality | |
| CN105278685B (en) | A kind of assisted teaching system and teaching method based on EON | |
| CN103778301A (en) | Mechanical arm simulation method based on virtual prototype technology | |
| Wan et al. | Realistic virtual hand modeling with applications for virtual grasping | |
| Braumann et al. | Real-time robot simulation and control for architectural design | |
| CN107967057A (en) | A kind of Virtual assemble teaching method based on Leap Motion | |
| Mei et al. | Application of augmented reality technology in industrial design | |
| Roach et al. | Computer aided drafting virtual reality interface | |
| Guo et al. | Skinning With Deformable Chunks. | |
| US7088377B2 (en) | System and method for designing, synthesizing and analyzing computer generated mechanisms | |
| Dettori et al. | Art Touch with CREATE haptic interface | |
| CN107738256A (en) | A kind of teach-by-doing apery teaching robot's programing system | |
| Bin et al. | Virtual Reality Design of industrial robot teaching based on unity3D | |
| CN110070777B (en) | Huchizhui fish skin painting simulation training system and implementation method | |
| Zhang | Development of 3D Modeling Parameter Design in Furniture Design | |
| Cai et al. | Development of machine tool simulation based on Zspace | |
| Hongwei et al. | Research on the Crucial Technology in Virtual Training System of Engineering Equipment | |
| Li et al. | Modeling and Animation of Realistic 3D Virtual Human Using Inertial-sensor | |
| Ott et al. | Organic shape modeling through haptic devices | |
| Lu et al. | Animating by example | |
| Jiang et al. | Design of Human-Computer Interaction Teaching System Based on Virtual Reality Technology | |
| Wesche | Conceptual free-form styling in virtual environments | |
| Pang | Display and Interaction Design of VR Technology in Environmental Design |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| C06 | Publication | ||
| PB01 | Publication | ||
| C10 | Entry into substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant | ||
| CF01 | Termination of patent right due to non-payment of annual fee | ||
| CF01 | Termination of patent right due to non-payment of annual fee |
Granted publication date: 20180316 |