CN107309873A - Mechanical arm motion control method and system - Google Patents
Mechanical arm motion control method and system Download PDFInfo
- Publication number
- CN107309873A CN107309873A CN201710470865.8A CN201710470865A CN107309873A CN 107309873 A CN107309873 A CN 107309873A CN 201710470865 A CN201710470865 A CN 201710470865A CN 107309873 A CN107309873 A CN 107309873A
- Authority
- CN
- China
- Prior art keywords
- mrow
- msub
- mechanical arm
- barrier
- control point
- 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.)
- Granted
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/1656—Programme controls characterised by programming, planning systems for manipulators
- B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
- B25J9/1666—Avoiding collision or forbidden zones
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Manipulator (AREA)
Abstract
The present invention relates to a kind of mechanical arm motion control method and system, method comprises the following steps：Obtain the real time position of the barrier between the Origin And Destination of manipulator motion；Each barrier and the beeline at previously selected control point on joint of mechanical arm are obtained according to the real time position, the beeline sum is calculated；The first fictitious force to the control point and the second fictitious force to mechanical arm terminal are calculated according to the movement velocity of the beeline sum and the joint of mechanical arm in real time, the real-time control moment to the mechanical arm is determined according to first fictitious force and the second fictitious force；Real-time route control is carried out to mechanical arm according to the real-time control moment.The above method and system-computed amount are small, and control efficiency is high.
Description
Technical field
The present invention relates to mechanical arm technical field, more particularly to a kind of mechanical arm motion control method and system.
Background technology
In the actual operative scenario of mechanical arm, there may be numerous barriers.In motion planning problem, it is necessary to
Between given starting point, terminal, the path of a collision free is cooked up, so as to ensure the safety of operation.
The motion planning of mechanical arm is divided into offline and online two ways.Offline mode can be before manipulator motion in advance
Operating path has been planned, but this needs to know in advance the geometry and position and attitude of barrier in working space；And it is online
Mode system by sensor while obtain real-time environmental data, and the synchronous motion to mechanical arm on one side is planned.Typically
For, offline mode efficiency is higher, but exists for some in the working environment of dynamic barrier, it is necessary to use online side
Formula.
Existing tool arm path On-line Control mode is usually to be moved according to the distribution real-time computer tool arm of barrier
Multiple intermediate points, according to the motion path of intermediate point control machinery arm, this mode amount of calculation is larger, causes manipulator motion control
System is less efficient.
The content of the invention
Based on this, it is necessary to for manipulator motion control efficiency it is relatively low the problem of there is provided a kind of control of manipulator motion
Method and system.
A kind of mechanical arm motion control method, comprises the following steps：
Obtain the real time position of the barrier between the Origin And Destination of manipulator motion；
Each barrier and the most short distance at previously selected control point on joint of mechanical arm are obtained according to the real time position
From the calculating beeline sum；
Calculated in real time to the control point according to the movement velocity of the beeline sum and the joint of mechanical arm
The first fictitious force and the second fictitious force to mechanical arm terminal, determined pair according to first fictitious force and the second fictitious force
The real-time control moment of the mechanical arm；
Real-time route control is carried out to mechanical arm according to the real-time control moment.
A kind of manipulator motion control system, including：
First acquisition module, the real time position of the barrier between Origin And Destination for obtaining manipulator motion；
Second acquisition module, for obtaining each barrier with being preselected on joint of mechanical arm according to the real time position
Control point beeline, calculate the beeline sum；
Computing module, for being calculated in real time according to the beeline sum and the movement velocity of the joint of mechanical arm
The first fictitious force to the control point and the second fictitious force to mechanical arm terminal, according to first fictitious force and second
Fictitious force determines the real-time control moment to the mechanical arm；
Path clustering module, for carrying out real-time route control to mechanical arm according to the real-time control moment.
Above-mentioned mechanical arm motion control method and system, can be obtained according to the real time position of barrier each barrier with
The beeline sum at previously selected control point on joint of mechanical arm, according to the beeline sum calculate the first fictitious force and
Second fictitious force, real-time control moment is generated according to the first fictitious force and the second fictitious force, and according to real-time control moment to machine
Tool arm carries out real-time route control, during manipulator motion, can be according to the generation of the distance between mechanical arm and barrier
One active force, pushes away barrier, it is to avoid mechanical arm knocks barrier by mechanical arm.The above method and system-computed amount are small, control
Efficiency high processed.
Brief description of the drawings
Fig. 1 is the mechanical arm motion control method flow chart of one embodiment；
Fig. 2 is the joint of mechanical arm of one embodiment and the schematic diagram at control point；
Fig. 3 is the programming flowchart of the mechanical arm motion control method of one embodiment；
Fig. 4 is the structural representation of the manipulator motion control system of one embodiment.
Embodiment
Technical scheme is illustrated below in conjunction with the accompanying drawings.
As shown in figure 1, the present invention provides a kind of mechanical arm motion control method, it may include following steps：
S1, obtains the real time position of the barrier between the Origin And Destination of manipulator motion；
Real time position in the present embodiment can be dynamic position, i.e. the real time position of each barrier can be with the time
Change.Refer to mechanical arm from starting point P between Origin And DestinationsMove to terminal PeMotion path on the position that may pass through.
In one embodiment, the real time position of barrier can be obtained by sensor, sensor can be each with certain frequency collection
The real time position of individual barrier.In a specific embodiment, the movement velocity of barrier can be obtained, according to the fortune of barrier
Dynamic speed sets the frequency of sensor collection real time position.For example, when the movement velocity of barrier is more than default threshold speed
When, can be with larger frequency collection real time position；Conversely, can be with less frequency collection real time position.Here " compared with
Greatly " and " smaller " can be relative to some acquiescence frequency acquisition value.Further, multiple threshold value shelves can also be set,
When the movement velocity of barrier is more than First Speed threshold value, real time position is gathered with first frequency；When the motion speed of barrier
When degree is more than second speed threshold value, real time position is gathered with second frequency；……；When the movement velocity of barrier is more than the n-th speed
When spending threshold value, using n-th (n is positive integer) frequency collection real time position；Wherein, first frequency, second frequency ..., the n-th frequency
Value be sequentially reduced.Different threshold speeds and corresponding frequency acquisition by setting, can both ensure that Obstacle Position was obtained
It is ageing, can reduce data interaction amount in the case where Obstacle Position conversion is relatively slow again.
S2, according to the real time position obtain each barrier with joint of mechanical arm previously selected control point it is most short
Distance, calculates the beeline sum；
Control point can be a point on joint of mechanical arm, and this point can be preselected.Each joint of mechanical arm
On a control point can be set.The joint of mechanical arm of one embodiment and the schematic diagram at control point are as shown in Fig. 2 mechanical arm
Including three joints, respectively there is a selected control point P on three joints1(Q)、P2And P (Q)3(Q), o is that mechanical arm work is empty
Between in barrier set, Peff(Q) it is the terminal location of mechanical arm, mechanical arm terminal refers in mechanical arm tail end, is used for and ring
The equipment of border interaction, is specifically dependent upon the application scenarios of robot.Than saying, one clip is installed in mechanical arm tail end, this
Clip is exactly the terminal of this mechanical arm.Parameter Q=[q1,q2,…,qn] represent the angle in mechanical arm each joint.To each control
It is processed, each barrier can be calculated respectively to the beeline sum at the control point.For i-th of control point, each barrier of t
Thing is hindered to be designated as to the beeline sum at the control point：
In formula, Di,tRepresent the beeline sum at each barrier of t and i-th of control point, PiRepresent i-th of machine
Control point on tool shoulder joint, Pt' represent t barrier real time position, oiRepresent that the work of i-th of joint of mechanical arm is empty
Between, k represents the number of joint of mechanical arm.
S3, is calculated to the control in real time according to the movement velocity of the beeline sum and the joint of mechanical arm
The first fictitious force and the second fictitious force to mechanical arm terminal of point, are determined according to first fictitious force and the second fictitious force
To the real-time control moment of the mechanical arm；
According to the dynamics of mechanical arm, the movement velocity of i-th of joint of mechanical arm in t cartesian spaceWith i-th
The angular speed of individual joint of mechanical armBetween there is following relation：
In formula, J is Jacobian matrix, and T represents that transposition is operated,To represent Pi(Q) to the derivative of time,For qiPair when
Between derivative, i=1,2 ..., k.
Similarly, the power F in the cartesian space and torque Π=[π in each joint of mechanical arm0,π1,…,πk] between deposit
In following relation：
F=J Π； (4)
Π=JTF； (5)
In t, for i-th of control point, following first fictitious force can be generated：
In formula, Fi,tFor first fictitious force of the t to i-th of control point, γ, c, b is default control parameter,For t
The movement velocity of i-th of joint of mechanical arm of moment, Di,tRepresent each barrier of t and i-th of control point beeline it
With.It is assumed that the angle q of i-th of joint of mechanical arm of tiAnd angular speed, it is known thatIt can be obtained by formula (2).
Further, in t, for mechanical arm terminal, following second fictitious force can be generated：
Wherein, Et=Pe-Peff,t；
In formula, Feff,tFor second fictitious force of the t to mechanical arm terminal, alpha, gamma, c, g, b is default control parameter,
PeFor the final position of manipulator motion, Peff,tFor the position of t mechanical arm terminal,For i-th of joint of mechanical arm of t
Movement velocity, Di,tThe beeline sum at each barrier of t and i-th of control point is represented, e is natural constant.
Correspondingly, in t, the real-time control moment of i-th of joint of mechanical arm can be designated as：
In formula, πtFor real-time control moment of the t to mechanical arm,For the corresponding Jacobi in i-th of control point of t
Matrix, Fi,tFor first fictitious force of the t to i-th of control point, Feff,tFor second fictitious force of the t to mechanical arm terminal,
K represents the number of joint of mechanical arm, and T represents that transposition is operated.
S4, real-time route control is carried out according to the real-time control moment to mechanical arm.
According to above-mentioned real-time control moment, each motor operation of mechanical arm can be driven, is realized to each joint of mechanical arm
Control.The programming flowchart of the mechanical arm motion control method of one embodiment is as shown in Figure 3.
By the above-mentioned means, the present invention can obtain each barrier and joint of mechanical arm according to the real time position of barrier
The beeline sum at upper previously selected control point, it is virtual according to the beeline sum the first fictitious force of calculating and second
Power, generates real-time control moment, and mechanical arm is carried out according to real-time control moment according to the first fictitious force and the second fictitious force
Real-time route is controlled, during manipulator motion, can be according to one effect of the distance between mechanical arm and barrier generation
Power, barrier is pushed away by mechanical arm, it is to avoid mechanical arm knocks barrier.
The present invention has advantages below：
(1) On-line Control mode is used, effectively mechanical arm can be entered in the working environment for exist dynamic barrier
Row motion control.
(2) intermediate point without the motion of calculating machine arm repeatedly, reduces amount of calculation, improves manipulator motion control
Efficiency.
As shown in figure 4, the present invention also provides a kind of manipulator motion control system, it may include：
First acquisition module 10, the real time position of the barrier between Origin And Destination for obtaining manipulator motion；
Real time position in the present embodiment can be dynamic position, i.e. the real time position of each barrier can be with the time
Change.Refer to mechanical arm from starting point P between Origin And DestinationsMove to terminal PeMotion path on the position that may pass through.
In one embodiment, the real time position of barrier can be obtained by sensor, sensor can be each with certain frequency collection
The real time position of individual barrier.In a specific embodiment, the movement velocity of barrier can be obtained, according to the fortune of barrier
Dynamic speed sets the frequency of sensor collection real time position.For example, when the movement velocity of barrier is more than default threshold speed
When, can be with larger frequency collection real time position；Conversely, can be with less frequency collection real time position.Here " compared with
Greatly " and " smaller " can be relative to some acquiescence frequency acquisition value.Further, multiple threshold value shelves can also be set,
When the movement velocity of barrier is more than First Speed threshold value, real time position is gathered with first frequency；When the motion speed of barrier
When degree is more than second speed threshold value, real time position is gathered with second frequency；……；When the movement velocity of barrier is more than the n-th speed
When spending threshold value, using n-th (n is positive integer) frequency collection real time position；Wherein, first frequency, second frequency ..., the n-th frequency
Value be sequentially reduced.Different threshold speeds and corresponding frequency acquisition by setting, can both ensure that Obstacle Position was obtained
It is ageing, can reduce data interaction amount in the case where Obstacle Position conversion is relatively slow again.
Second acquisition module 20, for obtaining each barrier with being selected in advance on joint of mechanical arm according to the real time position
The beeline at fixed control point, calculates the beeline sum；
Control point can be a point on joint of mechanical arm, and this point can be preselected.Each joint of mechanical arm
On a control point can be set.The joint of mechanical arm of one embodiment and the schematic diagram at control point are as shown in Fig. 2 mechanical arm
Including three joints, respectively there is a selected control point P on three joints1(Q)、P2And P (Q)3(Q), o is that mechanical arm work is empty
Between in barrier set, Peff(Q) it is the terminal location of mechanical arm, mechanical arm terminal refers in mechanical arm tail end, is used for and ring
The equipment of border interaction, is specifically dependent upon the application scenarios of robot.Than saying, one clip is installed in mechanical arm tail end, this
Clip is exactly the terminal of this mechanical arm.Parameter Q=[q1,q2,…,qn] represent the angle in mechanical arm each joint.To each control
It is processed, each barrier can be calculated respectively to the beeline sum at the control point.For i-th of control point, each barrier of t
Thing is hindered to be designated as to the beeline sum at the control point：
In formula, Di,tRepresent the beeline sum at each barrier of t and i-th of control point, PiRepresent i-th of machine
Control point on tool shoulder joint, Pt' represent t barrier real time position, oiRepresent that the work of i-th of joint of mechanical arm is empty
Between, k represents the number of joint of mechanical arm.
Computing module 30, by according to the movement velocity of the beeline sum and the joint of mechanical arm in real time based on
The first fictitious force to the control point and the second fictitious force to mechanical arm terminal are calculated, according to first fictitious force and the
Two fictitious forces determine the real-time control moment to the mechanical arm；
According to the dynamics of mechanical arm, the movement velocity of i-th of joint of mechanical arm in t cartesian spaceWith i-th
The angular speed of individual joint of mechanical armBetween there is following relation：
In formula, J is Jacobian matrix, and T represents that transposition is operated,To represent Pi(Q) to the derivative of time,For qiPair when
Between derivative, i=1,2 ..., k.
Similarly, the power F in the cartesian space and torque Π=[π in each joint of mechanical arm0,π1,…,πk] between deposit
In following relation：
F=J Π； (4)
Π=JTF； (5)
In t, for i-th of control point, following first fictitious force can be generated：
In formula, Fi,tFor first fictitious force of the t to i-th of control point, γ, c, b is default control parameter,For t
The movement velocity of i-th of joint of mechanical arm of moment, Di,tRepresent each barrier of t and i-th of control point beeline it
With.It is assumed that the angle q of i-th of joint of mechanical arm of tiAnd angular speed, it is known thatIt can be obtained by formula (2).
Further, in t, for mechanical arm terminal, following second fictitious force can be generated：
Wherein, Et=Pe-Peff,t；
In formula, Feff,tFor second fictitious force of the t to mechanical arm terminal, alpha, gamma, c, g, b is default control parameter,
PeFor the final position of manipulator motion, Peff,tFor the position of t mechanical arm terminal,For i-th of joint of mechanical arm of t
Movement velocity, Di,tThe beeline sum at each barrier of t and i-th of control point is represented, e is natural constant.
Correspondingly, in t, the real-time control moment of i-th of joint of mechanical arm can be designated as：
In formula, πtFor real-time control moment of the t to mechanical arm,For the corresponding Jacobi in i-th of control point of t
Matrix, Fi,tFor first fictitious force of the t to i-th of control point, Feff,tFor second fictitious force of the t to mechanical arm terminal,
K represents the number of joint of mechanical arm, and T represents that transposition is operated.
Path clustering module 40, for carrying out real-time route control to mechanical arm according to the real-time control moment.
According to above-mentioned real-time control moment, each motor operation of mechanical arm can be driven, is realized to each joint of mechanical arm
Control.The programming flowchart of the mechanical arm motion control method of one embodiment is as shown in Figure 3.
By the above-mentioned means, the present invention can obtain each barrier and joint of mechanical arm according to the real time position of barrier
The beeline sum at upper previously selected control point, it is virtual according to the beeline sum the first fictitious force of calculating and second
Power, generates real-time control moment, and mechanical arm is carried out according to real-time control moment according to the first fictitious force and the second fictitious force
Real-time route is controlled, during manipulator motion, can be according to one effect of the distance between mechanical arm and barrier generation
Power, barrier is pushed away by mechanical arm, it is to avoid mechanical arm knocks barrier.
The present invention has advantages below：
(1) On-line Control mode is used, effectively mechanical arm can be entered in the working environment for exist dynamic barrier
Row motion control.
(2) intermediate point without the motion of calculating machine arm repeatedly, reduces amount of calculation, improves manipulator motion control
Efficiency.
The mechanical arm motion control method of the manipulator motion control system of the present invention and the present invention is corresponded, above-mentioned
The technical characteristic and its advantage that the embodiment of mechanical arm motion control method is illustrated are applied to manipulator motion and control system
In the embodiment of system, hereby give notice that.
Represent in flow charts or logic and/or step described otherwise above herein, for example, being considered use
In the order list for the executable instruction for realizing logic function, it may be embodied in any computer-readable medium, for
Instruction execution system, device or equipment (such as computer based system including the system of processor or other can be held from instruction
The system of row system, device or equipment instruction fetch and execute instruction) use, or combine these instruction execution systems, device or set
It is standby and use.For the purpose of this specification, " computer-readable medium " can any can be included, store, communicate, propagate or pass
Defeated program is for instruction execution system, device or equipment or the dress for combining these instruction execution systems, device or equipment and using
Put.
The more specifically example (non-exhaustive list) of computer-readable medium includes following：With one or more wirings
Electrical connection section (electronic installation), portable computer diskette box (magnetic device), random access memory (RAM), read-only storage
(ROM), erasable edit read-only storage (EPROM or flash memory), fiber device, and portable optic disk is read-only deposits
Reservoir (CDROM).In addition, can even is that can be in the paper of printing described program thereon or other are suitable for computer-readable medium
Medium, because can then enter edlin, interpretation or if necessary with it for example by carrying out optical scanner to paper or other media
His suitable method is handled electronically to obtain described program, is then stored in computer storage.
It should be appreciated that each several part of the present invention can be realized with hardware, software, firmware or combinations thereof.Above-mentioned
In embodiment, the software that multiple steps or method can in memory and by suitable instruction execution system be performed with storage
Or firmware is realized.If, and in another embodiment, can be with well known in the art for example, realized with hardware
Any one of row technology or their combination are realized：With the logic gates for realizing logic function to data-signal
Discrete logic, the application specific integrated circuit with suitable combinational logic gate circuit, programmable gate array (PGA), scene
Programmable gate array (FPGA) etc..
In the description of this specification, reference term " one embodiment ", " some embodiments ", " example ", " specifically show
The description of example " or " some examples " etc. means to combine specific features, structure, material or the spy that the embodiment or example are described
Point is contained at least one embodiment of the present invention or example.In this manual, to the schematic representation of above-mentioned term not
Necessarily refer to identical embodiment or example.Moreover, specific features, structure, material or the feature of description can be any
One or more embodiments or example in combine in an appropriate manner.
Each technical characteristic of embodiment described above can be combined arbitrarily, to make description succinct, not to above-mentioned reality
Apply all possible combination of each technical characteristic in example to be all described, as long as however, the combination of these technical characteristics is not deposited
In contradiction, the scope of this specification record is all considered to be.
Embodiment described above only expresses the several embodiments of the present invention, and it describes more specific and detailed, but simultaneously
Can not therefore it be construed as limiting the scope of the patent.It should be pointed out that coming for one of ordinary skill in the art
Say, without departing from the inventive concept of the premise, various modifications and improvements can be made, these belong to the protection of the present invention
Scope.Therefore, the protection domain of patent of the present invention should be determined by the appended claims.
Claims (10)
1. a kind of mechanical arm motion control method, it is characterised in that comprise the following steps：
Obtain the real time position of the barrier between the Origin And Destination of manipulator motion；
Each barrier and the beeline at previously selected control point on joint of mechanical arm, meter are obtained according to the real time position
Calculate the beeline sum；
Calculate the to the control point in real time according to the movement velocity of the beeline sum and the joint of mechanical arm
One fictitious force and the second fictitious force to mechanical arm terminal, are determined to described according to first fictitious force and the second fictitious force
The real-time control moment of mechanical arm；
Real-time route control is carried out to mechanical arm according to the real-time control moment.
2. mechanical arm motion control method according to claim 1, it is characterised in that obtain each according to the real time position
The step of individual barrier and the beeline sum at default control point, includes：
The beeline sum of each barrier and default control point is obtained according to equation below：
<mrow>
<msub>
<mi>D</mi>
<mrow>
<mi>i</mi>
<mo>,</mo>
<mi>t</mi>
</mrow>
</msub>
<mo>=</mo>
<munderover>
<mo>&Sigma;</mo>
<mrow>
<mi>i</mi>
<mo>=</mo>
<mn>1</mn>
</mrow>
<mi>k</mi>
</munderover>
<msub>
<mi>min</mi>
<mrow>
<msup>
<mi>p</mi>
<mo>&prime;</mo>
</msup>
<mo>&Element;</mo>
<msub>
<mi>o</mi>
<mi>i</mi>
</msub>
</mrow>
</msub>
<mo>|</mo>
<mo>|</mo>
<msub>
<mi>P</mi>
<mi>i</mi>
</msub>
<mo>-</mo>
<msubsup>
<mi>P</mi>
<mi>t</mi>
<mo>&prime;</mo>
</msubsup>
<mo>|</mo>
<mo>|</mo>
<mo>;</mo>
</mrow>
In formula, Di,tRepresent the beeline sum at each barrier of t and i-th of control point, PiRepresent i-th of mechanical arm pass
Control point on section, Pt' represent t barrier real time position, oiRepresent the working space of i-th of joint of mechanical arm, k tables
Show the number of joint of mechanical arm.
3. mechanical arm motion control method according to claim 2, it is characterised in that according to the beeline sum with
The movement velocity of the joint of mechanical arm is calculated to first fictitious force at the control point and to mechanical arm terminal in real time
The step of second fictitious force, includes：
The first fictitious force to the control point is generated according to equation below：
<mrow>
<msub>
<mi>F</mi>
<mrow>
<mi>i</mi>
<mo>,</mo>
<mi>t</mi>
</mrow>
</msub>
<mo>=</mo>
<mi>&gamma;</mi>
<mo>&lsqb;</mo>
<mi>c</mi>
<msub>
<mover>
<mi>P</mi>
<mo>&CenterDot;</mo>
</mover>
<mrow>
<mi>i</mi>
<mo>,</mo>
<mi>t</mi>
</mrow>
</msub>
<mo>+</mo>
<msub>
<mi>bD</mi>
<mrow>
<mi>i</mi>
<mo>,</mo>
<mi>t</mi>
</mrow>
</msub>
<mo>&rsqb;</mo>
<mo>;</mo>
</mrow>
In formula, Fi,tFor first fictitious force of the t to i-th of control point, γ, c, b is default control parameter,For t
The movement velocity of i-th of joint of mechanical arm, Di,tRepresent the beeline sum at each barrier of t and i-th of control point.
4. the mechanical arm motion control method according to claims 1 to 3 any one, it is characterised in that according to it is described most
Short distance sum and the movement velocity of the joint of mechanical arm are calculated to first fictitious force at the control point and right in real time
The step of second fictitious force of mechanical arm terminal, also includes：
The second fictitious force to mechanical arm terminal is generated according to equation below：
<mrow>
<msub>
<mi>F</mi>
<mrow>
<mi>e</mi>
<mi>f</mi>
<mi>f</mi>
<mo>,</mo>
<mi>t</mi>
</mrow>
</msub>
<mo>=</mo>
<mo>-</mo>
<msub>
<mi>&alpha;E</mi>
<mi>t</mi>
</msub>
<mo>+</mo>
<mi>&gamma;</mi>
<mo>&lsqb;</mo>
<mi>c</mi>
<msub>
<mover>
<mi>P</mi>
<mo>&CenterDot;</mo>
</mover>
<mrow>
<mi>i</mi>
<mo>,</mo>
<mi>t</mi>
</mrow>
</msub>
<mo>+</mo>
<mi>g</mi>
<mi>f</mi>
<mrow>
<mo>(</mo>
<msub>
<mi>E</mi>
<mi>t</mi>
</msub>
<mo>)</mo>
</mrow>
<mo>+</mo>
<msub>
<mi>bD</mi>
<mrow>
<mi>i</mi>
<mo>,</mo>
<mi>t</mi>
</mrow>
</msub>
<mo>&rsqb;</mo>
<mo>;</mo>
</mrow>
Wherein, Et=Pe-Peff,t；
<mrow>
<mi>f</mi>
<mrow>
<mo>(</mo>
<msub>
<mi>E</mi>
<mi>t</mi>
</msub>
<mo>)</mo>
</mrow>
<mo>=</mo>
<mrow>
<mo>(</mo>
<mn>1</mn>
<mo>-</mo>
<msup>
<mi>e</mi>
<msub>
<mi>E</mi>
<mi>t</mi>
</msub>
</msup>
<mo>)</mo>
</mrow>
<mo>/</mo>
<mrow>
<mo>(</mo>
<mn>1</mn>
<mo>+</mo>
<msup>
<mi>e</mi>
<msub>
<mi>E</mi>
<mi>t</mi>
</msub>
</msup>
<mo>)</mo>
</mrow>
<mo>;</mo>
</mrow>
In formula, Feff,tFor second fictitious force of the t to mechanical arm terminal, alpha, gamma, c, g, b is default control parameter, PeFor
The final position of manipulator motion, Peff,tFor the position of t mechanical arm terminal,For i-th of joint of mechanical arm of t
Movement velocity, Di,tThe beeline sum at each barrier of t and i-th of control point is represented, e is natural constant.
5. mechanical arm motion control method according to claim 4, it is characterised in that according to first fictitious force and
The step of two fictitious forces determine the real-time control moment to the mechanical arm includes：
Real-time control moment to the mechanical arm is determined according to equation below：
<mrow>
<msub>
<mi>&pi;</mi>
<mi>t</mi>
</msub>
<mo>=</mo>
<mrow>
<mo>(</mo>
<munderover>
<mi>&Sigma;</mi>
<mrow>
<mi>i</mi>
<mo>=</mo>
<mn>1</mn>
</mrow>
<mi>k</mi>
</munderover>
<msubsup>
<mi>J</mi>
<mrow>
<mi>i</mi>
<mo>,</mo>
<mi>t</mi>
</mrow>
<mi>T</mi>
</msubsup>
<msub>
<mi>F</mi>
<mrow>
<mi>i</mi>
<mo>,</mo>
<mi>t</mi>
</mrow>
</msub>
<mo>)</mo>
</mrow>
<mo>+</mo>
<msubsup>
<mi>J</mi>
<mrow>
<mi>i</mi>
<mo>,</mo>
<mi>t</mi>
</mrow>
<mi>T</mi>
</msubsup>
<msub>
<mi>F</mi>
<mrow>
<mi>e</mi>
<mi>f</mi>
<mi>f</mi>
<mo>,</mo>
<mi>t</mi>
</mrow>
</msub>
<mo>;</mo>
</mrow>
In formula, πtFor real-time control moment of the t to mechanical arm,For the corresponding Jacobi square in i-th of control point of t
Battle array, Fi,tFor first fictitious force of the t to i-th of control point, Feff,tFor second fictitious force of the t to mechanical arm terminal, k
The number of joint of mechanical arm is represented, T represents that transposition is operated.
6. a kind of manipulator motion control system, it is characterised in that including：
First acquisition module, the real time position of the barrier between Origin And Destination for obtaining manipulator motion；
Second acquisition module, for obtaining each barrier and previously selected control on joint of mechanical arm according to the real time position
The beeline of point is made, the beeline sum is calculated；
Computing module, for being calculated in real time to institute according to the movement velocity of the beeline sum and the joint of mechanical arm
First fictitious force and the second fictitious force to mechanical arm terminal at control point are stated, it is virtual according to first fictitious force and second
Power determines the real-time control moment to the mechanical arm；
Path clustering module, for carrying out real-time route control to mechanical arm according to the real-time control moment.
7. manipulator motion control system according to claim 6, it is characterised in that second acquisition module is further
For：
The beeline sum of each barrier and default control point is obtained according to equation below：
<mrow>
<msub>
<mi>D</mi>
<mrow>
<mi>i</mi>
<mo>,</mo>
<mi>t</mi>
</mrow>
</msub>
<mo>=</mo>
<munderover>
<mo>&Sigma;</mo>
<mrow>
<mi>i</mi>
<mo>=</mo>
<mn>1</mn>
</mrow>
<mi>k</mi>
</munderover>
<msub>
<mi>min</mi>
<mrow>
<msup>
<mi>p</mi>
<mo>&prime;</mo>
</msup>
<mo>&Element;</mo>
<msub>
<mi>o</mi>
<mi>i</mi>
</msub>
</mrow>
</msub>
<mo>|</mo>
<mo>|</mo>
<msub>
<mi>P</mi>
<mi>i</mi>
</msub>
<mo>-</mo>
<msubsup>
<mi>P</mi>
<mi>t</mi>
<mo>&prime;</mo>
</msubsup>
<mo>|</mo>
<mo>|</mo>
<mo>;</mo>
</mrow>
In formula, Di,tRepresent the beeline sum at each barrier of t and i-th of control point, PiRepresent i-th of mechanical arm pass
Control point on section, Pt' represent t barrier real time position, oiRepresent the working space of i-th of joint of mechanical arm, k tables
Show the number of joint of mechanical arm.
8. manipulator motion control system according to claim 7, it is characterised in that the computing module is further used
In：
The first fictitious force to the control point is generated according to equation below：
<mrow>
<msub>
<mi>F</mi>
<mrow>
<mi>i</mi>
<mo>,</mo>
<mi>t</mi>
</mrow>
</msub>
<mo>=</mo>
<mi>&gamma;</mi>
<mo>&lsqb;</mo>
<mi>c</mi>
<msub>
<mover>
<mi>P</mi>
<mo>&CenterDot;</mo>
</mover>
<mrow>
<mi>i</mi>
<mo>,</mo>
<mi>t</mi>
</mrow>
</msub>
<mo>+</mo>
<msub>
<mi>bD</mi>
<mrow>
<mi>i</mi>
<mo>,</mo>
<mi>t</mi>
</mrow>
</msub>
<mo>&rsqb;</mo>
<mo>;</mo>
</mrow>
In formula, Fi,tFor first fictitious force of the t to i-th of control point, γ, c, b is default control parameter,For t
The movement velocity of i-th of joint of mechanical arm, Di,tRepresent the beeline sum at each barrier of t and i-th of control point.
9. the manipulator motion control system according to claim 6 to 8 any one, the computing module is further used
In：
The second fictitious force to mechanical arm terminal is generated according to equation below：
<mrow>
<msub>
<mi>F</mi>
<mrow>
<mi>e</mi>
<mi>f</mi>
<mi>f</mi>
<mo>,</mo>
<mi>t</mi>
</mrow>
</msub>
<mo>=</mo>
<mo>-</mo>
<msub>
<mi>&alpha;E</mi>
<mi>t</mi>
</msub>
<mo>+</mo>
<mi>&gamma;</mi>
<mo>&lsqb;</mo>
<mi>c</mi>
<msub>
<mover>
<mi>P</mi>
<mo>&CenterDot;</mo>
</mover>
<mrow>
<mi>i</mi>
<mo>,</mo>
<mi>t</mi>
</mrow>
</msub>
<mo>+</mo>
<mi>g</mi>
<mi>f</mi>
<mrow>
<mo>(</mo>
<msub>
<mi>E</mi>
<mi>t</mi>
</msub>
<mo>)</mo>
</mrow>
<mo>+</mo>
<msub>
<mi>bD</mi>
<mrow>
<mi>i</mi>
<mo>,</mo>
<mi>t</mi>
</mrow>
</msub>
<mo>&rsqb;</mo>
<mo>;</mo>
</mrow>
Wherein, Et=Pend-Peff,t；
<mrow>
<mi>f</mi>
<mrow>
<mo>(</mo>
<msub>
<mi>E</mi>
<mi>t</mi>
</msub>
<mo>)</mo>
</mrow>
<mo>=</mo>
<mrow>
<mo>(</mo>
<mn>1</mn>
<mo>-</mo>
<msup>
<mi>e</mi>
<msub>
<mi>E</mi>
<mi>t</mi>
</msub>
</msup>
<mo>)</mo>
</mrow>
<mo>/</mo>
<mrow>
<mo>(</mo>
<mn>1</mn>
<mo>+</mo>
<msup>
<mi>e</mi>
<msub>
<mi>E</mi>
<mi>t</mi>
</msub>
</msup>
<mo>)</mo>
</mrow>
<mo>;</mo>
</mrow>
In formula, Feff,tFor second fictitious force of the t to mechanical arm terminal, alpha, gamma, c, g, b is default control parameter, PendFor
The final position of manipulator motion, Peff,tFor the position of t mechanical arm terminal,For i-th of joint of mechanical arm of t
Movement velocity, Di,tThe beeline sum at each barrier of t and i-th of control point is represented, e is natural constant.
10. manipulator motion control system according to claim 9, it is characterised in that the computing module is further used
In：
Real-time control moment to the mechanical arm is determined according to equation below：
<mrow>
<msub>
<mi>&pi;</mi>
<mi>t</mi>
</msub>
<mo>=</mo>
<mrow>
<mo>(</mo>
<munderover>
<mi>&Sigma;</mi>
<mrow>
<mi>i</mi>
<mo>=</mo>
<mn>1</mn>
</mrow>
<mi>k</mi>
</munderover>
<msubsup>
<mi>J</mi>
<mrow>
<mi>i</mi>
<mo>,</mo>
<mi>t</mi>
</mrow>
<mi>T</mi>
</msubsup>
<msub>
<mi>F</mi>
<mrow>
<mi>i</mi>
<mo>,</mo>
<mi>t</mi>
</mrow>
</msub>
<mo>)</mo>
</mrow>
<mo>+</mo>
<msubsup>
<mi>J</mi>
<mrow>
<mi>i</mi>
<mo>,</mo>
<mi>t</mi>
</mrow>
<mi>T</mi>
</msubsup>
<msub>
<mi>F</mi>
<mrow>
<mi>e</mi>
<mi>f</mi>
<mi>f</mi>
<mo>,</mo>
<mi>t</mi>
</mrow>
</msub>
<mo>;</mo>
</mrow>
In formula, πtFor real-time control moment of the t to mechanical arm,For the corresponding Jacobi square in i-th of control point of t
Battle array, Fi,tFor first fictitious force of the t to i-th of control point, Feff,tFor second fictitious force of the t to mechanical arm terminal, k
The number of joint of mechanical arm is represented, T represents that transposition is operated.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201710470865.8A CN107309873B (en) | 2017-06-20 | 2017-06-20 | Mechanical arm motion control method and system |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201710470865.8A CN107309873B (en) | 2017-06-20 | 2017-06-20 | Mechanical arm motion control method and system |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN107309873A true CN107309873A (en) | 2017-11-03 |
| CN107309873B CN107309873B (en) | 2020-01-07 |
Family
ID=60181931
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201710470865.8A Active CN107309873B (en) | 2017-06-20 | 2017-06-20 | Mechanical arm motion control method and system |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN107309873B (en) |
Cited By (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107351085A (en) * | 2017-08-21 | 2017-11-17 | 西北工业大学 | A kind of robot for space collision avoidance method based on multiple control points |
| CN109571478A (en) * | 2018-12-17 | 2019-04-05 | 浙江大学昆山创新中心 | A kind of series connection multi-degree-of-freemechanical mechanical arm end tracking control method |
| CN109620410A (en) * | 2018-12-04 | 2019-04-16 | 微创（上海）医疗机器人有限公司 | The method and system of mechanical arm anticollision, medical robot |
| CN113276114A (en) * | 2021-05-20 | 2021-08-20 | 北京师范大学 | Reconfigurable mechanical arm cooperative force/motion control system and method based on terminal task assignment |
Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| DE102008016604A1 (en) * | 2008-04-01 | 2009-10-08 | Kuka Roboter Gmbh | Method for peculiar collision monitoring of manipulator, particularly robot, involves determining geometric body, which encloses element of manipulator |
| CN101628414A (en) * | 2009-08-17 | 2010-01-20 | 塔米智能科技（北京）有限公司 | Motion method of robot |
| CN105437232A (en) * | 2016-01-11 | 2016-03-30 | 湖南拓视觉信息技术有限公司 | Method and device for controlling multi-joint moving robot to avoid obstacle |
| CN106003043A (en) * | 2016-06-20 | 2016-10-12 | 先驱智能机械（深圳）有限公司 | Obstacle avoidance method and obstacle avoidance system of mechanical arm |
-
2017
- 2017-06-20 CN CN201710470865.8A patent/CN107309873B/en active Active
Patent Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| DE102008016604A1 (en) * | 2008-04-01 | 2009-10-08 | Kuka Roboter Gmbh | Method for peculiar collision monitoring of manipulator, particularly robot, involves determining geometric body, which encloses element of manipulator |
| CN101628414A (en) * | 2009-08-17 | 2010-01-20 | 塔米智能科技（北京）有限公司 | Motion method of robot |
| CN105437232A (en) * | 2016-01-11 | 2016-03-30 | 湖南拓视觉信息技术有限公司 | Method and device for controlling multi-joint moving robot to avoid obstacle |
| CN106003043A (en) * | 2016-06-20 | 2016-10-12 | 先驱智能机械（深圳）有限公司 | Obstacle avoidance method and obstacle avoidance system of mechanical arm |
Cited By (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107351085A (en) * | 2017-08-21 | 2017-11-17 | 西北工业大学 | A kind of robot for space collision avoidance method based on multiple control points |
| CN109620410A (en) * | 2018-12-04 | 2019-04-16 | 微创（上海）医疗机器人有限公司 | The method and system of mechanical arm anticollision, medical robot |
| CN109620410B (en) * | 2018-12-04 | 2021-01-26 | 微创（上海）医疗机器人有限公司 | Method and system for preventing collision of mechanical arm and medical robot |
| CN109571478A (en) * | 2018-12-17 | 2019-04-05 | 浙江大学昆山创新中心 | A kind of series connection multi-degree-of-freemechanical mechanical arm end tracking control method |
| CN109571478B (en) * | 2018-12-17 | 2021-07-27 | 浙江大学昆山创新中心 | Tracking control method for tail end of serially-connected multi-degree-of-freedom mechanical arm |
| CN113276114A (en) * | 2021-05-20 | 2021-08-20 | 北京师范大学 | Reconfigurable mechanical arm cooperative force/motion control system and method based on terminal task assignment |
Also Published As
| Publication number | Publication date |
|---|---|
| CN107309873B (en) | 2020-01-07 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110955242B (en) | Robot navigation method, system, robot and storage medium | |
| CN107309873A (en) | Mechanical arm motion control method and system | |
| Kang et al. | Planning and visualization for automated robotic crane erection processes in construction | |
| Ardiyanto et al. | Real-time navigation using randomized kinodynamic planning with arrival time field | |
| CN108582069A (en) | Robot drags teaching system and method, storage medium, operating system | |
| CN107608344B (en) | Vehicle motion control method and device based on trajectory planning and related equipment | |
| US9895803B1 (en) | Calculating trajectory corridor for robot end effector | |
| CN109291055B (en) | Robot motion control method, device, computer equipment and storage medium | |
| CN110316193A (en) | Setting method, device, equipment and the computer readable storage medium of preview distance | |
| CN106679669A (en) | Mobile robot path planning method and system | |
| CN102460330A (en) | Determination of routes for arranging automatic control of mobile mining machine | |
| CN103970139A (en) | Robot continuous point position motion planning method and motion controller thereof | |
| CN110000815A (en) | Collision detection method and device, electronic equipment and storage medium | |
| CN112904890A (en) | Unmanned aerial vehicle automatic inspection system and method for power line | |
| EP3847583A1 (en) | Determining control policies by minimizing the impact of delusion | |
| CN102034005B (en) | Shield machine posture simulation detection system for shield tunnel construction | |
| CN114815851A (en) | Robot following method, robot following device, electronic device, and storage medium | |
| CN105004368A (en) | Collision detection method, device and system for autonomous robot | |
| US10035264B1 (en) | Real time robot implementation of state machine | |
| CN113978465A (en) | Lane-changing track planning method, device, equipment and storage medium | |
| KR20190112644A (en) | Robot control apparatus, robot control method, and program | |
| CN113752300A (en) | Industrial robot energy consumption prediction method | |
| US20240009841A1 (en) | Dynamic target tracking method, robot and computer-readable storage medium | |
| CN105302156B (en) | A kind of method for planning track of ground validation system and pursuit spacecraft | |
| CN110549375A (en) | protective door anti-collision method and system for mechanical arm |
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 | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |