CN110091329A - A kind of cooperative motion control method of double mechanical arms - Google Patents
A kind of cooperative motion control method of double mechanical arms Download PDFInfo
- Publication number
- CN110091329A CN110091329A CN201910359484.1A CN201910359484A CN110091329A CN 110091329 A CN110091329 A CN 110091329A CN 201910359484 A CN201910359484 A CN 201910359484A CN 110091329 A CN110091329 A CN 110091329A
- Authority
- CN
- China
- Prior art keywords
- fpga
- mechanical arm
- coordinate
- control method
- motion control
- 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
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/1679—Programme controls characterised by the tasks executed
- B25J9/1682—Dual arm manipulator; Coordination of several manipulators
Abstract
The invention discloses a kind of cooperative motion control methods of double mechanical arms, belong to robot device's application field.The method includes initializing mechanical arm；The coordinate data of the mechanical arm tail end is inputted by host computer；FPGA receives coordinate data and is parsed；The FPGA plans the motion path of mechanical arm according to the coordinate data after parsing；The FPGA is parsed by inverse kinematics according to motion path and is obtained motor corner；The FPGA drives principal and subordinate's mechanical arm synchronizing moving according to motor corner.The present invention realizes the function of double mechanical arms cooperative motion, promotes data processing speed and working efficiency.
Description
Technical field
The invention belongs to robot device's application field, the cooperative motion control method of specially a kind of double mechanical arms.
Background technique
It is mostly the operation of Single Mechanical arm in current domestic industry production, Single Mechanical arm can substitute people to a certain extent
Work, but in work such as the assembly of weight or like parts, the polishings for facing geometry complexity, it appears it feels at a loss.
It is domestic in recent years to start to have at present on a small quantity successively about the research of more mechanical arm control modes based on neural network and based on more
The research data of intelligent body, it is each to have certain theoretical foundation and simulation result support by oneself, but there are mechanical arms for the control of more mechanical arms
Between cooperate with poor, or even the problem of can not cooperate with, also need further to study apart from practical application and demonstration.
Summary of the invention
In view of the deficiencies of the prior art, the purpose of the present invention is to provide a kind of cooperative motion controlling parties of double mechanical arms
Method can be only done simple motion to solve Single Mechanical arm exists in the prior art, and more mechanical arms can not be cooperateed with and be worked
Problem.
To achieve the above object, the technical scheme adopted by the invention is as follows:
A kind of cooperative motion control method of double mechanical arms, the described method comprises the following steps:
Initialize mechanical arm；
The coordinate data of the mechanical arm tail end is inputted by host computer；
FPGA receives coordinate data and is parsed；
The FPGA plans the motion path of mechanical arm according to the coordinate data after parsing；
The FPGA is parsed by inverse kinematics according to motion path and is obtained motor corner；
The FPGA drives principal and subordinate's mechanical arm synchronizing moving according to motor corner.
Further, the mechanical arm initial method includes:
The FPGA electrification reset；
Each motor on FPGA con current control principal and subordinate's mechanical arm；
Specific square wave is exported by the motor, each motor corner is made to reach intermediate value；
Judge whether motor is in normal condition according to the middle finger；
It is ready that principal and subordinate's mechanical arm is moved to initial position, waits coordinate to be received.
The initial position can be adjusted voluntarily as needed.
Further, the input method of the coordinate data includes:
Establish the communication connection between host computer and FPGA；
Data between the upper computer selecting and FPGA send agreement；
Coordinate data is packaged into data packet by the host computer, and is sent to FPGA.
FPGA can be connect by bluetooth or data line with host computer, and state machine enters ready state after successful connection,
Upper computer selecting communication protocol, wherein coordinate data agreement is generated by Background control, operator it is not necessary to modify its format and content,
After the selected one or more coordinates of upper computer end, host computer is wrapped into data packet, is sent to FPGA, and FPGA has received institute
It is parsed after having coordinate, picks out coordinate information, prepare output pulse signal, state machine enters motion state.
Further, communication protocol includes port, baud rate, check bit, data bit and coordinate data format.
Further, the trajectory path planning method includes:
The FPGA constructs motion vector according to the coordinate data after parsing；
Run duration is obtained according to the motion vector calculation；
The run duration is infinitely segmented and obtains a series of space coordinate i.e. motion path.
The motion vector is point-to-point spacing straight line.
The run duration determines that movement velocity can independently be selected according to the actual situation by operator according to movement velocity.
A series of space coordinates are that the spacing straight line being calculated is passed through.
When FPGA parses to obtain one or more coordinate informations, whole story vector is obtained by calculation in inside, according to regulation
The time required to movement is calculated in speed, run duration is infinitely segmented, a series of space coordinate is obtained.For mechanical arm
Speech, pulse signal mutation can generate biggish electric current in moment, generate acceleration, after leading to manipulator motion to purpose coordinate,
It nevertheless suffers from inertia effects and generates zigzag motion profile, amplitude is excessive to cause the loss of electric machine in order to avoid driving signal is mutated
And the influence to track, FPGA infinitely segment coordinate in operation, motion process are divided into several continuous movements, to subtract
The influence of light inertia and acceleration.The selection of the movement velocity is reasonable, guarantee mechanical arm can at work slowly at the uniform velocity
Movement.
Further, the method for the motor corner acquisition includes:
Coordinate system is established according to principal and subordinate's mechanical arm position
Inverse Kinematics solution formula is constructed according to coordinate system；
The space coordinate of motion path is resolved into each motor corner according to inverse kinematics formula.
Each control unit sequentially stores related data flow.
Principal and subordinate's mechanical arm position should fully consider mechanical structure.
Each motor corresponds to a control unit in mechanical arm, and FPGA is parsed after obtaining a succession of coordinate, according to inverse fortune
Coordinate is successively resolved to each motor corner by dynamic formula of learning, and each motor angle information is sequentially stored in control unit again, produces
Raw queue, after state machine enters motion state, the data flow in queue sequentially enters control unit to driving motor, inverse fortune
The dynamic calculating for learning formula, belongs to existing knowledge, this will not be repeated here.
Further, the synchronizing moving method includes:
Motor corner is converted into driving signal, FPGA drives principal and subordinate's mechanical arm synchronizing moving according to driving signal.
Further, the host computer is PC or mobile phone.
Further, the mechanical arm is Cartesian robot arm, cylindrical coordinates mechanical arm or polar coordinates mechanical arm；The machine
The freedom degree of tool arm is more than or equal to 4.
Further, the method also includes:
The driving signal in movement is constantly acquired during the manipulator motion；
Driving signal in the movement is converted into coordinate information；
By the coordinate information and initial coordinate information comparison；
The driving signal in movement is constantly regulate according to comparison information.
The influence for needing to fully consider mechanical structure is chosen in the position of principal and subordinate's mechanical arm, selects reasonable reference position
Computational complexity is advantageously reduced, the speed of service and efficiency are improved.
By drive module, driving signal successively is converted by corner, driving motor is constantly taken out during moving
Sample, the resulting driving signal that will sample, which just solves, is converted to coordinate information, which is compared with initial coordinate, according to difference tune
Joint number maintains the information such as relative position, constantly compares it with the coordinates of motion according to the speed by control unit, initial coordinate,
It can detecte and violate collaboration fortune since disturbance or other X factors cause that movement velocity is inconsistent and relative position is inconsistent etc.
The problem of dynamic agreement, so that mechanical arm be avoided to generate irreversible damage during exercise.
The movement agreement defines requirement most basic in mechanical arm cooperative motion, including location consistency, speed one
Cause property and non-static consistency etc..
The driving signal is typically the square-wave signal of particular duty cycle, and particular technique details is determined according to motor model
It is fixed.
Compared with prior art, the present invention have it is following the utility model has the advantages that
The present invention realizes double mechanical arms by FPGA Collaborative Control double mechanical arms motor using the concurrent processing characteristic of FPGA
The function of cooperative motion promotes data processing speed and working efficiency；FPGA can receive feedback letter while motion control
Breath carries out kinematics equations and feedback information calculation process, clock delay needed for reducing solving kinematic equation.
Detailed description of the invention
Fig. 1 is a kind of flow chart of the cooperative motion control method specific embodiment of double mechanical arms of the present invention.
Fig. 2 is mechanical arm initialization process in a kind of cooperative motion control method specific embodiment of double mechanical arms of the present invention
Figure.
Fig. 3 is that FPGA obtains ending coordinates in a kind of cooperative motion control method specific embodiment of double mechanical arms of the present invention
Flow chart.
Fig. 4 is FPGA programming movement path in a kind of cooperative motion control method specific embodiment of double mechanical arms of the present invention
Flow chart.
Fig. 5 is inverse kinematics equations in a kind of cooperative motion control method specific embodiment of double mechanical arms of the present invention
Corner flow chart.
Fig. 6 is that FPGA drives mechanical arm stream in a kind of cooperative motion control method specific embodiment of double mechanical arms of the present invention
Cheng Tu.
Specific embodiment
Following will be combined with the drawings in the embodiments of the present invention, and technical solution in the embodiment of the present invention carries out clear, complete
Site preparation description, it is clear that described embodiments are only a part of the embodiments of the present invention, instead of all the embodiments.It is based on
Embodiment in the present invention, those of ordinary skill in the art without making creative work it is obtained it is all its
His embodiment, shall fall within the protection scope of the present invention.
As shown in Figure 1, Figure 2, shown in Fig. 3, Fig. 4, Fig. 5, Fig. 6, a kind of cooperative motion control method of double mechanical arms, this method packet
It includes:
S10, initialization mechanical arm: the FPGA electrification reset；Each electricity on FPGA con current control principal and subordinate's mechanical arm
Machine；Specific square wave is exported by the motor, each motor corner is made to reach intermediate value；Whether motor is judged according to the middle finger
In normal condition；It is ready that principal and subordinate's mechanical arm is moved to initial position.
S20, ending coordinates are obtained: establishes the communication connection between host computer and FPGA；The upper computer selecting and FPGA
Between data send agreement；Coordinate data is packaged into data packet by the host computer, and is sent to FPGA.
S30, programming movement path: FPGA receives coordinate data and is parsed, and the FPGA is according to the coordinate after parsing
Data construct motion vector；Run duration is obtained according to the motion vector calculation；The run duration is infinitely segmented and is obtained
A series of space coordinate, that is, motion path.
S40, Inverse Kinematics parsing obtain corner；Coordinate system is established according to principal and subordinate's mechanical arm position, and fortune is constructed according to coordinate system
It is dynamic to learn inverse solution formula；The space coordinate of motion path is resolved into each motor corner according to inverse kinematics formula.
S50, FPGA drive mechanical arm: the motor corner being converted to driving signal, FPGA drives according to driving signal
Principal and subordinate's mechanical arm synchronizing moving.
Specifically, the object that is handled upside down of constant weight can be fixed using existing sixdegree-of-freedom simulation on the market
In mechanical arm tail end, deformable object is selected for fixing material, is avoided during exercise due to there is subtle unknown disturbance
Cause the inconsistent situation of principal and subordinate's mechanical arm terminal position occur to damage mechanical arm.The position selection of principal and subordinate's mechanical arm should
Most easy principle is followed, that is, when establishing inverse kinematics model solution, can reduce and be produced since reference coordinate is chosen to greatest extent
Raw computational complexity, mechanical arm is placed on same plane in the present embodiment, and is respectively in the diagonally opposing corner of square, meets:
X1=R-X2；Y1=R-Y2, to mitigate computational complexity to a certain extent.Mobile phone or the end PC, the present embodiment may be selected in host computer
It is middle that data are sent using the end PC, by sending one or more coordinates in upper computer end, control principal and subordinate's manipulator motion.Mechanical arm
It can be controlled by development board or FPGA, the signal and control mechanical arm fortune that host computer is sent are received using FPGA in the present embodiment
It is dynamic.
S10 specifically includes the following steps:
S101, FPGA electrification reset；
S102, building coordinate system；
S103, each unit export intermediate value；
S104, abnormality detection；
S105, alarm signal；
S106, into initial position.
FPGA exports med signal and is arranged according to the mechanical arm model of selection, and there be specific account in the generally period equal to 20ms
The square-wave signal of empty ratio, if it is possible to make motor reach corner and not detect that stall etc. is abnormal, that is, can be moved into initial position
Into ready state, initial position is by operator's self-setting.
S20 specifically includes the following steps:
S201, detection connection；S202, judge whether to connect；
S203, determine whether response；
S204, connection is established；
S205, selection agreement；
S206, judge whether coordinate is effective；
S207, give up coordinate；
S208, coordinate is sent.
The no special language requirement of the programming of host computer, it is desirable to provide port, the options such as baud rate are different to cope with
It is necessary to have an input windows for communication protocol, are capable of the spatial position of input coordinate system, and coordinate data is packaged into data
Packet.Selecting for coordinate system can be rectangular coordinate system or polar coordinate system.Transport protocol selection is mainly reflected in selection port, baud
The conversion of rate and parity check bit etc., coordinate data is provided with processing by internal agreement, does not meet the seat of data protocol specification
Mark will be cast out, and the coordinate information of specification is finally packaged into data packet by way of data line or wireless transmission and is sent
To FPGA.
S30 specifically includes the following steps:
S301, FPGA received data packet；
S302, judge that coordinate is effective；
S303, give up coordinate；
S304, storage coordinate；
S305, building motion vector；
S306, run duration is calculated；
S307, space coordinate is generated.
FPGA receives and parses data packet according to given transmission agreement, extracts coordinate information, judges the effective of coordinate one by one
Property, the coordinate other than space coordinates, which violates agreement, to be cast out automatically, legal one or more coordinates are finally obtained, the
N and (n+1)th coordinate building motion vector, which is point-to-point spacing straight line, and FPGA calculates the fortune according to movement velocity
The time required to dynamic, run duration is then subdivided into N number of time quantum and is substituted into motion vector, obtains each time quantum
Space coordinate, a series of obtained space coordinates can approximately be thought to the path of the object of which movement.
S40 specifically includes the following steps:
S401, inverse kinematics module is built；
S402, input coordinate, S403, each unit store corner.
The con current control of FPGA can actually regard each unit concurrent working by same clock along triggering, Ge Gedan as
Member includes address data memory and drive module.Inverse kinematics angle is existing knowledge, and this will not be repeated here.
S50 specifically includes the following steps:
S501, setting clock module；
S02, each unit input angular signal；
S503, using driving signal；
S504, coordinate is just solved；
S505, judge coordinate consistency.
The coordinate of each corner of FPGA parsing gained can be sequentially stored into the memory module of each unit, when state machine enters
Ready state, each unit are successively read coordinate according to clock signal, by the specific square-wave signal of drive module final output, use
To drive corner.Clock is determined according to movement velocity, it may be necessary to make adjusting.In order to avoid subtle unknown disturbance is to fortune
Dynamic stability impacts, and the output signal of each drive module can be sampled constantly, and just solves to obtain principal and subordinate's mechanical arm
Coordinate voluntarily adjusts clock frequency according to the positional relationship of coordinate, and the relative position of principal and subordinate's arm is finally made as to keep permanent as possible
It is fixed.
The collaborative work of more mechanical arms can make up such deficiency, imitate out the working method of more people, can carry,
Greater advantages are played in the work such as component assembly.
The above-mentioned a certain application that the invention is only illustrated with example, aspect reader understands, but does not represent purposes of the present invention
It is only limitted to this, for the non-technical personnel of this field, it is possible to understand that in the case of without departing from the principles of the present invention, Ke Yi
Make extension or recreation on the basis of this, the scope of the present invention is subject to claims.
Claims (10)
1. a kind of cooperative motion control method of double mechanical arms, which is characterized in that the described method comprises the following steps:
Initialize mechanical arm；
The coordinate data of the mechanical arm tail end is inputted by host computer；
FPGA receives coordinate data and is parsed；
The FPGA plans the motion path of mechanical arm according to the coordinate data after parsing；
The FPGA is parsed by inverse kinematics according to motion path and is obtained motor corner；
The FPGA drives principal and subordinate's mechanical arm synchronizing moving according to motor corner.
2. a kind of cooperative motion control method of double mechanical arms according to claim 1, which is characterized in that the mechanical arm
Initial method includes:
The FPGA electrification reset；
Each motor on FPGA con current control principal and subordinate's mechanical arm；
Specific square wave is exported by the motor, each motor corner is made to reach intermediate value；
Judge whether motor is in normal condition according to the intermediate value；
It is ready that principal and subordinate's mechanical arm is moved to initial position.
3. a kind of cooperative motion control method of double mechanical arms according to claim 1, which is characterized in that the number of coordinates
According to input method include:
Establish the communication connection between host computer and FPGA；
Data between the upper computer selecting and FPGA send agreement；
Coordinate data is packaged into data packet by the host computer, and is sent to FPGA.
4. a kind of cooperative motion control method of double mechanical arms according to claim 3, which is characterized in that the communication protocols
View includes port, baud rate, check bit, data bit and coordinate data format.
5. a kind of cooperative motion control method of double mechanical arms according to claim 1, which is characterized in that the movement road
Diameter planing method includes:
The FPGA constructs motion vector according to the coordinate data after parsing；
Run duration is obtained according to the motion vector calculation；
The run duration is infinitely segmented and obtains a series of space coordinate i.e. motion path.
6. a kind of cooperative motion control method of double mechanical arms according to claim 1, which is characterized in that the motor turns
Angle obtain method include:
Coordinate system is established according to principal and subordinate's mechanical arm position
Inverse Kinematics solution formula is constructed according to coordinate system；
The space coordinate of motion path is resolved into each motor corner according to inverse kinematics formula.
7. a kind of cooperative motion control method of double mechanical arms according to claim 1, which is characterized in that the synchronous shifting
Dynamic method includes:
The motor corner is converted into driving signal, FPGA drives principal and subordinate's mechanical arm synchronizing moving according to driving signal.
8. a kind of cooperative motion control method of double mechanical arms according to claim 1, which is characterized in that the host computer
For PC or mobile phone.
9. a kind of cooperative motion control method of double mechanical arms according to claim 1, which is characterized in that the mechanical arm
For Cartesian robot arm, cylindrical coordinates mechanical arm or polar coordinates mechanical arm；The freedom degree of the mechanical arm is more than or equal to 4.
10. a kind of cooperative motion control method of double mechanical arms according to claim 1, which is characterized in that the method
Further include:
The driving signal in movement is constantly acquired during the manipulator motion；
Driving signal in the movement is converted into coordinate information；
By the coordinate information and initial coordinate information comparison；
The driving signal in movement is constantly regulate according to comparison information.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910359484.1A CN110091329A (en) | 2019-04-30 | 2019-04-30 | A kind of cooperative motion control method of double mechanical arms |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910359484.1A CN110091329A (en) | 2019-04-30 | 2019-04-30 | A kind of cooperative motion control method of double mechanical arms |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN110091329A true CN110091329A (en) | 2019-08-06 |
Family
ID=67446454
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201910359484.1A Pending CN110091329A (en) | 2019-04-30 | 2019-04-30 | A kind of cooperative motion control method of double mechanical arms |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN110091329A (en) |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103273489A (en) * | 2013-05-10 | 2013-09-04 | 上海大学 | Robot control system and method based on principal and subordinate teleoperation mechanical arm |
| CN104385273A (en) * | 2013-11-22 | 2015-03-04 | 嘉兴市德宝威微电子有限公司 | Robot system and synchronous performance control method thereof |
| CN106873447A (en) * | 2017-01-10 | 2017-06-20 | 南开大学 | A kind of robot model's algorithm implementation method based on FPGA |
| CN107030679A (en) * | 2017-05-31 | 2017-08-11 | 贾相晟 | A kind of master slave mode teaching system and teaching method for mechanical arm |
| CN108638073A (en) * | 2018-06-08 | 2018-10-12 | 杭州电子科技大学 | The six shaft mechanical arm point-to-point spacing straight lines based on FPGA plan implementation method |
-
2019
- 2019-04-30 CN CN201910359484.1A patent/CN110091329A/en active Pending
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103273489A (en) * | 2013-05-10 | 2013-09-04 | 上海大学 | Robot control system and method based on principal and subordinate teleoperation mechanical arm |
| CN104385273A (en) * | 2013-11-22 | 2015-03-04 | 嘉兴市德宝威微电子有限公司 | Robot system and synchronous performance control method thereof |
| US20160346920A1 (en) * | 2013-11-22 | 2016-12-01 | Jiaxing Doublewell Microelectronics. Ltd | Robot system and simultaneous performance control method thereof |
| CN106873447A (en) * | 2017-01-10 | 2017-06-20 | 南开大学 | A kind of robot model's algorithm implementation method based on FPGA |
| CN107030679A (en) * | 2017-05-31 | 2017-08-11 | 贾相晟 | A kind of master slave mode teaching system and teaching method for mechanical arm |
| CN108638073A (en) * | 2018-06-08 | 2018-10-12 | 杭州电子科技大学 | The six shaft mechanical arm point-to-point spacing straight lines based on FPGA plan implementation method |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Andersson et al. | Simulation of wireless networked control systems | |
| CN107214701B (en) | A kind of livewire work mechanical arm automatic obstacle avoiding paths planning method based on movement primitive library | |
| CN110320873A (en) | A kind of real-time three-dimensional presentation system based on distributed sensor | |
| CN106182027B (en) | A kind of open service robot system | |
| CN108287933B (en) | Intelligent assembly type building hoisting system and method based on BIM and virtual reality technology | |
| CN106060058A (en) | Internet-based industrial robot remote control method | |
| CN109782600A (en) | A method of autonomous mobile robot navigation system is established by virtual environment | |
| CN108733062A (en) | Family accompanies and attends to robot autonomous charging system and method | |
| CN106078752A (en) | Method is imitated in a kind of anthropomorphic robot human body behavior based on Kinect | |
| CN202453710U (en) | Multi-axis linkage numerical control system | |
| CN106313049A (en) | Somatosensory control system and control method for apery mechanical arm | |
| CN102163381A (en) | Unmanned aerial vehicle electric power pipeline walking simulation training system | |
| CN102798834A (en) | Wiring judgement control machine for wiring simulation system for verification on power transformer | |
| CN109434808A (en) | A kind of cloud remote service Study of Intelligent Robot Control network system realization | |
| CN106411184A (en) | Networked multi-axis motor synchronization control device and method | |
| CN108214445A (en) | A kind of principal and subordinate's isomery remote operating control system based on ROS | |
| CN108983636A (en) | Human-machine intelligence's symbiosis plateform system | |
| CN103413487A (en) | Transformer assembling technology interactive simulation system and method | |
| CN104858852A (en) | Optimization and constraint method of humanoid robot for real-time imitating human upper limb movement | |
| CN111220999A (en) | Restricted space detection system and method based on instant positioning and mapping technology | |
| CN107483284A (en) | The method of testing and device of the network equipment | |
| CN106200448B (en) | A kind of long-range mapped system of industry interface implementation | |
| CN110091329A (en) | A kind of cooperative motion control method of double mechanical arms | |
| CN104932385A (en) | Multi-axis control system and method for wave simulation | |
| CN112800606A (en) | Digital twin production line construction method and system, electronic device and storage medium |
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: 20190806 |