CN108772835A - A kind of barrier hides method with physics limit - Google Patents
A kind of barrier hides method with physics limit Download PDFInfo
- Publication number
- CN108772835A CN108772835A CN201810412706.7A CN201810412706A CN108772835A CN 108772835 A CN108772835 A CN 108772835A CN 201810412706 A CN201810412706 A CN 201810412706A CN 108772835 A CN108772835 A CN 108772835A
- Authority
- CN
- China
- Prior art keywords
- barrier
- vector
- limit
- physics limit
- constraint
- 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
-
- 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/1607—Calculation of inertia, jacobian matrixes and inverses
Abstract
The invention discloses a kind of barriers and physics limit to hide method, includes the following steps：According to the physical model of goal systems, row write its kinematical equation expression formula；Solve the Jacobian matrix of goal systems end effector；According to the relationship between goal systems and barrier, the constraint of its Obstacle avoidance is defined；Obstacle avoidance constraint is optimized, the Obstacle avoidance method based on vector is designed；According to the physical model of goal systems, its physics limit constraint expression formula is set；Obstacle avoidance method based on vector is merged with physics limit constraint expression formula, and combines the Jacobian matrix of end effector, the barrier based on vector of design object system hides method with physics limit；It is converted to obtain the linear differential inequality of equal value with it；It is converted to obtain the linear projection equation of equal value with it；Linear projection equation substitution primal-dual neural network solver is resolved, you can complete goal systems and barrier and physics limit are hidden.
Description
Technical field
The present invention relates to system controls and planning technology field, and in particular to a kind of barrier and the physics limit side of hiding
Method.
Background technology
It is in the modern intelligence system analysis research equipped with redundant mechanical arm that Obstacle avoidance and physics limit, which are hidden,
One of underlying issue is related to redundant mechanical arm success mounted and completes given end task.Therefore, dynamic is advised
Draw in control barrier and physics limit to hide be extremely important and with practical significance.If not accounting for above-mentioned
Barrier and physics limit are hidden, and work failure will likely be caused even to cause the damage of mechanical arm.
Traditional method is the method based on Artificial Potential Field, its basic thought is the fortune in ambient enviroment by mechanical arm
It is dynamic, it is designed to a kind of abstract movement in artificial gravitational field, target point generates " gravitation " to mobile mechanical arm, barrier
" repulsion " is generated to mobile mechanical arm, the movement of mobile mechanical arm is controlled finally by resultant force is sought.It is cooked up using potential field method
The path come is general smoother and safe, but this method has local best points.In addition to this, using this
When a kind of method of sample carries out Obstacle avoidance, it is desirable that barrier is preferably rule, and otherwise the calculation amount of algorithm will be very big, sometimes
It is even imponderable.The inherent limitation of method based on Artificial Potential Field is mainly manifested in：When target proximity has obstacle
When object, mechanical arm will not reach destination always.In pervious many researchs, target and barrier all from it is far,
When mechanical arm approaches target, the very little of the repulsion change of barrier, it might even be possible to ignore, mechanical arm will only be made by attraction
With and through target.But in many actual environments, often at least one barrier and target point from it is close, in this feelings
Under condition, while mobile mechanical arm approaches target, it also will be close to barrier, if using in the past to gravitation field function and
The definition of repulsion field function, repulsion will be bigger than gravitation more, such target point by be not entire potential field global minima point, therefore
Mechanical arm would be impossible to reach target.Thus there are problems that locally optimal solution, therefore how to design " gravitational field " problem just
As the key of this method.Meanwhile not considering that the physics of redundant mechanical arm itself closes while this method of application at present
Limits properties are saved, a series of infeasible problems such as mechanical arm is locked are easy to cause when actually executing task.
In face of such a background, it would be highly desirable to propose a kind of barrier based on vector and physics limit hide method and to
On mobile platform of the reality equipped with redundant mechanical arm.
Invention content
The purpose of the present invention is to solve drawbacks described above in the prior art, provide a kind of barrier based on vector with
Physics limit hides method.
The purpose of the present invention can be reached by adopting the following technical scheme that：
A kind of barrier hides method with physics limit, and the method for hiding includes the following steps：
S1, the physical model according to goal systems, row write its kinematical equation expression formula；
S2, the kinematical equation expression formula based on goal systems in step S1 solve the refined of goal systems end effector
Gram compare matrix；
Relationship between S3, foundation goal systems and barrier defines the constraint of its Obstacle avoidance；
S4, the Obstacle avoidance constraint described in step S3 is optimized, designs the Obstacle avoidance side based on vector
Method；
S5, the physical model according to goal systems, set its physics limit constraint expression formula；
S6, by the Obstacle avoidance method based on vector described in step S4 with the physics limit described in step S5 about
Beam expression formula merges, and combines the Jacobian matrix of the end effector described in step S2, design object system based on arrow
The barrier of amount hides method with physics limit；
S7, by based on vector described in step S6 barrier and physics limit hide method and convert, obtain with
Its linear differential inequality form of equal value；
S8, the linear differential inequality described in step S7 is converted, obtains the linear projection equation of equal value with it
Form；
S9, the linear projection equation substitution primal-dual neural network solver described in step S8 is resolved, you can
Goal systems is completed to hide barrier and physics limit.
Further, according to the physical model of goal systems, row write following kinematical equation expression formula：
Wherein, which is movement of the working space in the carrying 6DOF redundant mechanical arm of three dimensional coordinate space
Plateform system；MatrixTotal output relationship for describing the system；MatrixFor the kinetic model of mobile platform；
MatrixFor the kinetic model of mechanical arm；For the position vector of robot arm end effector known to hypothesis.
MatrixIt is defined as follows：
Wherein, φ is the course angle of mobile platform；X, y, z is respectively the location information of three-dimensional coordinate.
MatrixIt is defined as follows：
Wherein,WithFor the homogeneous transform matrix of the 6DOF redundant mechanical arm.
Further, according to kinematical equation expression formula (1), solution obtains the refined of following goal systems end effector
Gram compare matrix：
Wherein, θ is the joint angle variable of joint of mechanical arm.
Further, according to the relationship between goal systems and barrier, the Obstacle avoidance being defined as follows constrains：
Wherein,It is defined as：
Wherein, m indicates the dimension of working space, when m=3 epoch entry mark systems are operated under three dimensions；Parameter σ tables
It is shown with effect obstacle object point and logarithm of the critical point on joint of mechanical arm angle； Indicate the Jacobi square of critical point
Battle array；It is defined as follows：
Wherein, (xc,yc,zc) and (xo,yo,zo) be respectively critical point and obstacle object point coordinate；Symbol sgn [] is indicated
Sign function；Operator ⊙ is defined as：
Wherein, b=[b1,b2,…,bl] it is a column vector；For a matrix.
Further, the step S4 is specific as follows：
Since the right end that Obstacle avoidance constrains (5) is 0, may cause to stop in emergency when mechanical arm is close to barrier,
Mechanical arm is damaged, therefore (5) can be constrained to Obstacle avoidance and be optimized as follows：
Wherein, variableIt is defined as：
Smoothing equation S () is defined as：
Wherein, d is the distance between critical point and obstacle object point；Parameter d1And d2It is each set to exit critical distance
With into critical distance.
It notices when the distance between critical point and obstacle object point are too small, activation barrier is not enough to above-mentioned relation
Hide constraint (9), i.e., at this timeMeet following relationship：
In order to preferably utilize feasible domain space, by Obstacle avoidance constraint (9) advanced optimize, obtain it is following based on
The Obstacle avoidance method of vector：
Wherein,WithIt is defined as foloows：
DesignAs the velocity vector of goal systems critical point, then the left side of inequality (13) is expanded as following shape
Formula：
If formula (16) is negative, i.e.,Then vectorAnd vectorBetween angle will be less than or
Equal to 90 degree namely critical point will be moved away from the direction of obstacle object point.In other words, such a barrier based on vector
Entire feasible domain space can be used by hindering object to hide method, and be not limited solely to meet between critical point and obstacle object point away from
From more than a certain range of space.
Further, the step S5 is specific as follows：
According to the physical model of goal systems, its physics limit constraint expression formula is set.In order to ensure in the task of execution
The safety of redundant mechanical arm in goal systems sets following joint of mechanical arm angle limit and joint angle speed limit：
θ-≤θ≤θ+ (17)
Wherein, θ-And θ+Lower limit constraint and the upper limit constraint of joint angles θ are indicated respectively；WithJoint is indicated respectively
Angular speedLower limit constraint and the upper limit constraint.
Hide problem with physics limit due to the barrier of goal systems to solve on velocity layer, therefore angle restriction
(17) it will be transformed on velocity layer and parse with angular speed constraint (18).A kind of physics limit constrained procedure is set as follows：
Wherein, lower limit constrainsIt is constrained with the upper limitIt is respectively defined as：
Wherein, parameter k>0 is adjustment factor.By physics limit constraint (19) know, when its approaching limit of a joint angle about
When beam value, its angular speed can drop to 0 by limitation, to stopping before making joint angle reach physics limit or reversely.
Further, the step S6 is specific as follows：
The Obstacle avoidance method based on vector is merged with the physics limit constrained procedure, and combines institute
The Jacobian matrix for the goal systems end effector stated, design obtain a kind of based on the barrier of vector hiding with physics limit
Method.One kind have both Obstacle avoidance method (13), physics limit constrained procedure (19) and end effector Jacobian matrix
(4) the barrier based on vector hides method with physics limit, has following form：
Further, the step S7 is specific as follows：
Barrier based on vector is hidden method (22)-(25) with physics limit to convert, is obtained and its equivalence
Linear differential inequality form.Solution formula (22)-(25) are equivalent to search out a former Dual balance vectorIt meets following relationship：
Wherein,Indicate former antithesis decision variable；Indicate equation
Constrain the antithesis decision variable of (23)；Indicate the antithesis decision variable of inequality constraints (24)；Ω is convex as one
Set is defined as foloows：
Wherein, lower limit u-With upper limit u+Respectively：
ε > > 0 are set to the greatest extent
It may be greatly for replacement+∞；VectorAnd other parameters are defined as：
Wherein, E is the unit matrix of n × n；Its dependent variable is consistent with aforementioned.
Further, the step S8 is specific as follows：
Above-mentioned linear differential inequality (26) is transformed into the linear projection equation with following form：
Wherein,For fromProject to the linear projection operator of set omega；MatrixU andWith it is preceding
Definition is stated to be consistent.
Further, the step S9 is specific as follows：
Above-mentioned linear projection equation (36) is substituted into following primal-dual neural network solver to resolve：
The data obtained output will be resolved, you can complete goal systems and hide to barrier and physics limit.
The present invention has the following advantages and effects with respect to the prior art：
A kind of barrier disclosed by the invention is hidden method with physics limit and is asked based on processing quadratic programming emerging at present
The method of topic calculates the distance between barrier, and applies speed limit to mechanical arm well, to ensure it to barrier
Hinder hiding for object.At the same time, this method has fully considered the physics joint limit of practical redundant mechanical arm, and as pole
Limit constraint is added in quadratic programming expression formula, while completing Obstacle avoidance, ensures that mechanical arm is operated in rational object
It manages within limit range.Further, such a method can extend to similar equipped with different model redundant mechanical
The mobile platform system of arm, the barrier and physics limit for carrying out goal systems hide task.
Description of the drawings
Fig. 1 is the flow chart that barrier disclosed by the invention and physics limit hide method；
Fig. 2 (a) is the obstacle object point and pursuit path result figure of emulation experiment one in the case of barrier-avoiding method is not used；
Fig. 2 (b) is the obstacle object point and pursuit path result figure of emulation experiment one in the case of using traditional barrier-avoiding method；
Fig. 2 (c) is the obstacle object point and pursuit path using emulation experiment one in the case of the barrier-avoiding method based on vector
Result figure；
Fig. 3 (a) is the three-dimensional avoidance result figure of emulation experiment one in the case of barrier-avoiding method is not used；
Fig. 3 (b) is the three-dimensional avoidance result figure of emulation experiment one in the case of using traditional barrier-avoiding method；
Fig. 3 (c) is the three-dimensional avoidance result figure using emulation experiment one in the case of the barrier-avoiding method based on vector；
Fig. 4 (a) is the critical point of emulation experiment one and obstacle object point distance results figure in the case of barrier-avoiding method is not used；
Fig. 4 (b) is the critical point of emulation experiment one and obstacle object point distance results in the case of using traditional barrier-avoiding method
Figure；
Fig. 4 (c) be using emulation experiment one in the case of the barrier-avoiding method based on vector critical point and obstacle object point away from
From result figure；
Fig. 5 (a) is the norm error result figure of emulation experiment one in the case of barrier-avoiding method is not used；
Fig. 5 (b) is the norm error result figure of emulation experiment one in the case of using traditional barrier-avoiding method；
Fig. 5 (c) is the norm error result figure using emulation experiment one in the case of the barrier-avoiding method based on vector；
Fig. 6 (a) is the obstacle object point and pursuit path result figure of emulation experiment two in the case of barrier-avoiding method is not used；
Fig. 6 (b) is the obstacle object point and pursuit path result figure of emulation experiment two in the case of using traditional barrier-avoiding method；
Fig. 6 (c) is the obstacle object point and pursuit path using emulation experiment two in the case of the barrier-avoiding method based on vector
Result figure；
Fig. 7 (a) is the three-dimensional avoidance result figure of emulation experiment two in the case of barrier-avoiding method is not used；
Fig. 7 (b) is the three-dimensional avoidance result figure of emulation experiment two in the case of using traditional barrier-avoiding method；
Fig. 7 (c) is the three-dimensional avoidance result figure using emulation experiment two in the case of the barrier-avoiding method based on vector；
Fig. 8 (a) is the critical point of emulation experiment two and obstacle object point distance results figure in the case of barrier-avoiding method is not used；
Fig. 8 (b) is the critical point of emulation experiment two and obstacle object point distance results in the case of using traditional barrier-avoiding method
Figure；
Fig. 8 (c) be using emulation experiment two in the case of the barrier-avoiding method based on vector critical point and obstacle object point away from
From result figure.
Specific implementation mode
In order to make the object, technical scheme and advantages of the embodiment of the invention clearer, below in conjunction with the embodiment of the present invention
In attached drawing, technical scheme in the embodiment of the invention is clearly and completely described, it is clear that described embodiment is
A part of the embodiment of the present invention, instead of all the embodiments.Based on the embodiments of the present invention, those of ordinary skill in the art
The every other embodiment obtained without making creative work, shall fall within the protection scope of the present invention.
Embodiment
Fig. 1 show the flow chart that the barrier of present example hides method with physics limit, should barrier based on vector
Hinder object to hide method with physics limit to include the following steps：
S1, the physical model according to goal systems, row write its kinematical equation expression formula；
S2, the kinematical equation expression formula based on goal systems in step S1 solve the refined of goal systems end effector
Gram compare matrix；
Relationship between S3, foundation goal systems and barrier defines the constraint of its Obstacle avoidance；
S4, the Obstacle avoidance constraint described in step S3 is optimized, designs the Obstacle avoidance side based on vector
Method；
S5, the physical model according to goal systems, set its physics limit constraint expression formula；
S6, by the Obstacle avoidance method based on vector described in step S4 with the physics limit described in step S5 about
Beam expression formula merges, and combines the Jacobian matrix of the end effector described in step S2, design object system based on arrow
The barrier of amount hides method with physics limit；
S7, by based on vector described in step S6 barrier and physics limit hide method and convert, obtain with
Its linear differential inequality form of equal value；
S8, the linear differential inequality described in step S7 is converted, obtains the linear projection equation of equal value with it
Form；
S9, the linear projection equation substitution primal-dual neural network solver described in step S8 is resolved, you can
Goal systems is completed to hide barrier and physics limit.
It is respectively barrier-avoiding method to be not used, using traditional barrier-avoiding method and using based on vector shown in Fig. 2 (a)-Fig. 2 (c)
Barrier-avoiding method in the case of, the obstacle object point and pursuit path result figure of emulation experiment one.In emulation experiment one, barrier
Point is (0.45, -0.05,0.85), and end orbit is Circular test.
It is respectively barrier-avoiding method to be not used, using traditional barrier-avoiding method and using based on vector shown in Fig. 3 (a)-Fig. 3 (c)
Barrier-avoiding method in the case of, the three-dimensional avoidance result figure of emulation experiment one.By Fig. 3 (a) it is found that when barrier-avoiding method is not used,
Redundant mechanical arm collides with barrier, mission failure.By Fig. 3 (b) and Fig. 3 (c) it is found that using traditional barrier-avoiding method and making
When with barrier-avoiding method based on vector, redundant mechanical arm successfully avoids barrier, Mission Success.
It is respectively barrier-avoiding method to be not used, using traditional barrier-avoiding method and using based on vector shown in Fig. 4 (a)-Fig. 4 (c)
Barrier-avoiding method in the case of, the critical point of emulation experiment one and obstacle object point distance results figure.By Fig. 4 (a) it is found that being not used
When barrier-avoiding method, critical point occurs with obstacle object point spacing from less than collision distance, namely collision, mission failure.By Fig. 4 (b)
With Fig. 4 (c) it is found that using traditional barrier-avoiding method and when using the barrier-avoiding method based on vector, critical point and obstacle object point spacing
From more than collision distance, namely barrier is avoided, Mission Success.
It is respectively barrier-avoiding method to be not used, using traditional barrier-avoiding method and using based on vector shown in Fig. 5 (a)-Fig. 5 (c)
Barrier-avoiding method in the case of, the norm error result figure of emulation experiment one.By Fig. 5 (a) it is found that when barrier-avoiding method is not used,
The norm error of task execution is 0.0006435；By Fig. 5 (b) it is found that when using traditional barrier-avoiding method, the norm of task execution
Error is 0.1079；By Fig. 5 (c) it is found that when using barrier-avoiding method based on vector, the norm error of task execution is
0.0009241.In summary three results are it is found that norm error when using barrier-avoiding method of the present invention based on vector
Norm error when traditional barrier-avoiding method is far smaller than used, namely is also achieved very while completing avoidance using this method
High precision.
It is respectively barrier-avoiding method to be not used, using traditional barrier-avoiding method and using based on vector shown in Fig. 6 (a)-Fig. 6 (c)
Barrier-avoiding method in the case of, the obstacle object point and pursuit path result figure of emulation experiment two.In emulation experiment two, barrier
Point is (0.6,0.3,0.8), and end orbit is Lissajous trajectory.
It is respectively barrier-avoiding method to be not used, using traditional barrier-avoiding method and using based on vector shown in Fig. 7 (a)-Fig. 7 (c)
Barrier-avoiding method in the case of, the three-dimensional avoidance result figure of emulation experiment two.By Fig. 7 (a) and Fig. 7 (b) it is found that avoidance is not used
When method and use tradition barrier-avoiding method, redundant mechanical arm collides with barrier, mission failure.By Fig. 7 (c) it is found that making
When with barrier-avoiding method based on vector, redundant mechanical arm successfully avoids barrier, Mission Success.In conclusion in face of multiple
When miscellaneous track task, conventional method may lead to mission failure due to its limitation；And it is of the present invention based on vector
Barrier-avoiding method improves this defect, and feasible domain space can be fully utilized and carry out avoidance, embody its preferable applicability.
It is respectively barrier-avoiding method to be not used, using traditional barrier-avoiding method and using based on vector shown in Fig. 8 (a)-Fig. 8 (c)
Barrier-avoiding method in the case of, the critical point of emulation experiment two and obstacle object point distance results figure.It can by Fig. 8 (a) and Fig. 8 (b)
Know, barrier-avoiding method be not used and when using traditional barrier-avoiding method, critical point and obstacle object point spacing from less than collision distance, namely
Collision occurs, mission failure.By Fig. 8 (c) it is found that when using barrier-avoiding method based on vector, critical point and obstacle object point spacing
From more than collision distance, namely barrier is avoided, Mission Success.In conclusion when in face of complicated track task, tradition
Method may lead to mission failure due to its limitation；And the barrier-avoiding method of the present invention based on vector improves this
Defect can be fully utilized feasible domain space and carry out avoidance, embody its preferable applicability.
According to the correlation step of design flow diagram, it is directed to the present invention herein and carries out detailed arithmetic analysis.First, according to mesh
The physical model of mark system can arrange and write following kinematical equation expression formula：
Wherein, which is movement of the working space in the carrying 6DOF redundant mechanical arm of three dimensional coordinate space
Plateform system；MatrixTotal output relationship for describing the system；MatrixFor the kinetic model of mobile platform；
MatrixFor the kinetic model of mechanical arm；For the position vector of robot arm end effector known to hypothesis.
MatrixIt is defined as follows：
Wherein, φ is the course angle of mobile platform；X, y, z is respectively the location information of three-dimensional coordinate.
MatrixIt is defined as follows：
Wherein,WithFor the homogeneous transform matrix of the 6DOF redundant mechanical arm.
According to kinematical equation expression formula (1), can solve to obtain the Jacobi of following goal systems end effector
Matrix：
According to the relationship between goal systems and barrier, the Obstacle avoidance that can be defined as follows constrains：
Wherein,It is defined as：
M indicates the dimension of working space, when m=3 epoch entry mark systems are operated under three dimensions；Parameter σ is indicated
Imitate obstacle object point and logarithm of the critical point on joint of mechanical arm angle；Indicate the Jacobian matrix of critical point；It is defined as follows：
Wherein, (xc,yc,zc) and (xo,yo,zo) be respectively critical point and obstacle object point coordinate；Symbol sgn [] is indicated
Sign function；Operator ⊙ is defined as：
B=[b1,b2,…,bl] it is a column vector；For a matrix.
Since the right end that Obstacle avoidance constrains (5) is 0, may cause to stop in emergency when mechanical arm is close to barrier,
Mechanical arm is damaged, therefore (5) can be constrained to Obstacle avoidance and be optimized as follows：
Wherein, variableIt is defined as：
Smoothing equation S () is defined as：
D is the distance between critical point and obstacle object point；Parameter d1And d2It is each set to exit critical distance and entrance
Critical distance.
It notices when the distance between critical point and obstacle object point are too small, activation barrier is not enough to above-mentioned relation
Hide constraint (9), i.e., at this timeMeet following relationship：
In order to preferably utilize feasible domain space, by Obstacle avoidance constraint (9) advanced optimize, obtain it is following based on
The Obstacle avoidance method of vector：
Wherein,WithIt is defined as foloows：
DesignAs the velocity vector of goal systems critical point, then the left side of inequality (13) can be expanded as such as
Lower form：
If formula (16) is negative, i.e.,Then vectorAnd vectorBetween angle will be less than or
Equal to 90 degree namely critical point will be moved away from the direction of obstacle object point.In other words, such a barrier based on vector
Entire feasible domain space can be used by hindering object to hide method, and be not limited solely to meet between critical point and obstacle object point away from
From more than a certain range of space.
According to the physical model of goal systems, its physics limit constraint expression formula can be set.In order to ensure to appoint in execution
When business in goal systems redundant mechanical arm safety, set following joint of mechanical arm angle limit and joint angle speed limit：
θ-≤θ≤θ+ (17)
Wherein, θ-And θ+Lower limit constraint and the upper limit constraint of joint angles θ are indicated respectively；WithJoint is indicated respectively
Angular speedLower limit constraint and the upper limit constraint.
Hide problem with physics limit due to the barrier of goal systems to solve on velocity layer, therefore angle restriction
(17) it will be transformed on velocity layer and parse with angular speed constraint (18).A kind of physics limit constrained procedure is set as follows：
Wherein, lower limit constrainsIt is constrained with the upper limitIt is respectively defined as：
Wherein, parameter k>0 is adjustment factor.By physics limit constraint (19) know, when its approaching limit of a joint angle about
When beam value, its angular speed can drop to 0 by limitation, to stopping before making joint angle reach physics limit or reversely.
One kind have both Obstacle avoidance method (13), physics limit constrained procedure (19) and end effector Jacobi square
Battle array (4) is hidden method with physics limit based on the barrier of vector and is devised in the present invention, with following form：
Barrier based on vector is hidden method (22)-(25) with physics limit to convert, can obtain with its etc.
The linear differential inequality form of valence.Solution formula (22)-(25) are equivalent to search out a former Dual balance vectorIt meets following relationship：
Wherein,Indicate former antithesis decision variable；Indicate equation
Constrain the antithesis decision variable of (23)；Indicate the antithesis decision variable of inequality constraints (24)；Ω is convex as one
Set is defined as foloows：
Wherein, lower limit u-With upper limit u+Respectively：
ε > > 0 are set to the greatest extent
It may be greatly for replacement+∞；VectorAnd other parameters are defined as：
Wherein, E is the unit matrix of n × n；Its dependent variable is consistent with aforementioned.
Linear differential inequality (26) above-mentioned can be converted into the linear projection equation with following form：
Wherein,For fromProject to the linear projection operator of set omega；MatrixU andWith it is preceding
Definition is stated to be consistent.
Linear projection equation (36) above-mentioned can be substituted into following primal-dual neural network solver and be resolved：
The data obtained output will be resolved, you can complete goal systems and hide to barrier and physics limit.Herein, in order to
The actual application for showing the method for the invention, illustrates described problem using a simulation example.
This simulation example is based on a mobile platform equipped with 6DOF redundant mechanical arm.
Wherein, the joint angles in the initial joint of 6, redundant mechanical arm are set to：
θ (0)=[262.6,260.0,86.0,228.0,104.0,138.0]T；
The limit that exits in formula (11) is respectively set at 0.10m and 0.05m with into the limit；Parameter beta in formula (37)
It is set to 1 × 104m.Norm error when task execution be defined as on tri- coordinates of X, Y, Z the norm of error and, i.e.,The simulation example is divided into 2 two experiments of emulation experiment one and emulation experiment.
Barrier-avoiding method is not used, using traditional barrier-avoiding method and using in the case of the barrier-avoiding method based on vector, emulates
Shown in the obstacle object point and pursuit path result such as Fig. 2 (a)-Fig. 2 (c) of experiment one.In emulation experiment one, obstacle object point is
(0.45, -0.05,0.85), end orbit is Circular test.Barrier-avoiding method is not used, is based on using traditional barrier-avoiding method and use
In the case of the barrier-avoiding method of vector, shown in three-dimensional avoidance result such as Fig. 3 (a)-Fig. 3 (c) of emulation experiment one.It can by Fig. 3 (a)
Know, when barrier-avoiding method is not used, redundant mechanical arm collides with barrier, mission failure.By Fig. 3 (b) and Fig. 3 (c) it is found that
When using traditional barrier-avoiding method and using barrier-avoiding method based on vector, redundant mechanical arm successfully avoids barrier, task at
Work(.Barrier-avoiding method is not used, using traditional barrier-avoiding method and in the case of using the barrier-avoiding method based on vector, emulation experiment one
Critical point and obstacle object point distance results such as Fig. 4 (a)-Fig. 4 (c) shown in.By Fig. 4 (a) it is found that barrier-avoiding method is not used
When, critical point occurs with obstacle object point spacing from less than collision distance, namely collision, mission failure.By Fig. 4 (b) and Fig. 4 (c)
It is found that when using traditional barrier-avoiding method and using barrier-avoiding method based on vector, critical point and obstacle object point spacing are from more than touching
Distance is hit, namely avoids barrier, Mission Success.Barrier-avoiding method is not used, using traditional barrier-avoiding method and using based on arrow
In the case of the barrier-avoiding method of amount, shown in norm error result such as Fig. 5 (a)-Fig. 5 (c) of emulation experiment one.It can by Fig. 5 (a)
Know, when barrier-avoiding method is not used, the norm error of task execution is 0.0006435；By Fig. 5 (b) it is found that using traditional avoidance side
When method, the norm error of task execution is 0.1079；By Fig. 5 (c) it is found that when using barrier-avoiding method based on vector, task is held
Capable norm error is 0.0009241.In summary three results are it is found that use the avoidance side of the present invention based on vector
Norm error when norm error when method is far smaller than using traditional barrier-avoiding method, namely complete avoidance using this method
Also achieve very high precision simultaneously.
Barrier-avoiding method is not used, using traditional barrier-avoiding method and using in the case of the barrier-avoiding method based on vector, emulates
Shown in the obstacle object point and pursuit path result such as Fig. 6 (a)-Fig. 6 (c) of experiment two.In emulation experiment two, obstacle object point is
(0.6,0.3,0.8), end orbit are Lissajous trajectory.Barrier-avoiding method is not used, is based on using traditional barrier-avoiding method and use
In the case of the barrier-avoiding method of vector, shown in three-dimensional avoidance result such as Fig. 7 (a)-Fig. 7 (c) of emulation experiment two.By Fig. 7 (a) and
Fig. 7 (b) is it is found that when being not used barrier-avoiding method and using traditional barrier-avoiding method, and redundant mechanical arm collides with barrier, task
Failure.By Fig. 7 (c) it is found that when using barrier-avoiding method based on vector, redundant mechanical arm successfully avoids barrier, task at
Work(.In conclusion when in face of complicated track task, conventional method may lead to mission failure due to its limitation；And this
The invention barrier-avoiding method based on vector improves this defect, and feasible domain space can be fully utilized and carry out avoidance, body
Reveal its preferable applicability.Barrier-avoiding method is not used, using traditional barrier-avoiding method and using the barrier-avoiding method based on vector
In the case of, the critical point of emulation experiment two and distance results such as Fig. 8 (a)-Fig. 8 (c) of obstacle object point are shown.By Fig. 8 (a) and figure
8 (b) it is found that barrier-avoiding method is not used and when using traditional barrier-avoiding method, critical point and obstacle object point spacing from less than collision away from
From, namely collision generation, mission failure.By Fig. 8 (c) it is found that when using barrier-avoiding method based on vector, critical point and barrier
Distance is more than collision distance between point, namely avoids barrier, Mission Success.In conclusion in face of complicated track task
When, conventional method may lead to mission failure due to its limitation；And the barrier-avoiding method of the present invention based on vector improves
This defect can be fully utilized feasible domain space and carry out avoidance, embody its preferable applicability.
The present invention above-mentioned each arithmetic analysis step be only to clearly illustrate example of the present invention, and not be
Restriction to embodiments of the present invention.For those of ordinary skill in the art, on the basis of the above description also
It can make other variations or changes in different ways.There is no necessity and possibility to exhaust all the enbodiments.It is all
All any modification, equivalent and improvement made by within the spirit and principles in the present invention etc. should be included in right of the present invention and want
Within the protection domain asked.
Claims (10)
1. a kind of barrier hides method with physics limit, which is characterized in that the method for hiding includes the following steps：
S1, the physical model according to goal systems, row write its kinematical equation expression formula；
S2, the kinematical equation expression formula based on goal systems solve the Jacobian matrix of goal systems end effector；
Relationship between S3, foundation goal systems and barrier defines the constraint of its Obstacle avoidance；
S4, the Obstacle avoidance constraint is optimized, designs the Obstacle avoidance method based on vector；
S5, the physical model according to goal systems, set its physics limit constraint expression formula；
S6, the Obstacle avoidance method based on vector is merged with the physics limit constraint expression formula, and combined
The Jacobian matrix of the end effector, barrier based on vector and the physics limit side of hiding of design object system
Method；
S7, it the barrier based on vector and physics limit is hidden into method converts, obtain of equal value linear with it
Differential inequality form；
S8, the linear differential inequality is converted, obtains the linear projection equation form of equal value with it；
S9, the linear projection equation substitution primal-dual neural network solver is resolved, you can complete goal systems
Barrier and physics limit are hidden.
2. a kind of barrier according to claim 1 hides method with physics limit, which is characterized in that the kinematics
Equation expression formula is as follows：
Wherein, which is mobile platform of the working space in the carrying 6DOF redundant mechanical arm of three dimensional coordinate space
System；MatrixTotal output relationship for describing the system；MatrixFor the kinetic model of mobile platform；MatrixFor the kinetic model of mechanical arm；For the position vector of robot arm end effector known to hypothesis；
The matrixIt is defined as follows：
Wherein, φ is the course angle of mobile platform；X, y, z is respectively the location information of three-dimensional coordinate；
The matrixIt is defined as follows：
Wherein,WithFor the homogeneous transform matrix of the 6DOF redundant mechanical arm.
3. a kind of barrier according to claim 2 hides method with physics limit, which is characterized in that the step S2
It is as follows：
According to kinematical equation expression formula (1), solution obtains the Jacobian matrix of following goal systems end effector：
Wherein, θ is the joint angle variable of joint of mechanical arm.
4. a kind of barrier according to claim 3 hides method with physics limit, which is characterized in that the step S3
It is as follows：
According to the relationship between goal systems and barrier, the Obstacle avoidance being defined as follows constrains：
Wherein,It is defined as：
Wherein, m indicates the dimension of working space；Parameter σ indicates effective obstacle object point with critical point on joint of mechanical arm angle
Logarithm；Indicate the Jacobian matrix of critical point；It is defined as follows：
Wherein, (xc,yc,zc) and (xo,yo,zo) be respectively critical point and obstacle object point coordinate；Symbol sgn [] indicates symbol
Function；Operator ⊙ is defined as：
Wherein, b=[b1,b2,…,bl] it is a column vector；For a matrix.
5. a kind of barrier according to claim 4 hides method with physics limit, which is characterized in that the step S4
It is as follows：
Obstacle avoidance constraint (5) is optimized as follows：
Wherein, variableIt is defined as：
Smoothing equation S () is defined as：
Wherein, d is the distance between critical point and obstacle object point；Parameter d1And d2Be each set to exit critical distance and into
Enter critical distance；
It notices when the distance between critical point and obstacle object point are too small, activation Obstacle avoidance is not enough to above-mentioned relation
It constrains (9), i.e., at this timeMeet following relationship：
In order to preferably utilize feasible domain space, Obstacle avoidance constraint (9) is optimized, is obtained following based on vector
Obstacle avoidance method：
Wherein,WithIt is defined as foloows：
DesignAs the velocity vector of goal systems critical point, then the left side of inequality (13) is expanded as following form：
If formula (16) is negative, i.e.,Then vectorAnd vectorBetween angle will be less than or equal to 90
Degree namely critical point will be moved away from the direction of obstacle object point.
6. a kind of barrier according to claim 5 hides method with physics limit, which is characterized in that the step S5
It is as follows：
According to the physical model of goal systems, its physics limit constraint expression formula is set, in order to ensure the target in the task of execution
The safety of redundant mechanical arm in system sets following joint of mechanical arm angle limit and joint angle speed limit：
θ-≤θ≤θ+ (17)
Wherein, θ-And θ+Lower limit constraint and the upper limit constraint of joint angles θ are indicated respectively；WithJoint angular speed is indicated respectivelyLower limit constraint and the upper limit constraint；
Hide problem with physics limit due to the barrier of goal systems to solve on velocity layer, therefore angle restriction (17)
It will be transformed on velocity layer and parse with angular speed constraint (18), a kind of physics limit constrained procedure is set as follows：
Wherein, lower limit constrainsIt is constrained with the upper limitIt is respectively defined as：
Wherein, parameter k>0 is adjustment factor, is known by physics limit constraint (19), when its approaching limit restraint value of a joint angle
When, its angular speed can drop to 0 by limitation, to stopping before making joint angle reach physics limit or reversely.
7. a kind of barrier according to claim 6 hides method with physics limit, which is characterized in that described based on arrow
The barrier of amount hides method with physics limit has following form：
8. a kind of barrier according to claim 7 hides method with physics limit, which is characterized in that the step S7
It is as follows：
Barrier based on vector is hidden method Chinese style (22)-(25) with physics limit to convert, is obtained and its equivalence
Linear differential inequality form；Solution formula (22)-(25) are equivalent to search out a former Dual balance vectorIt meets following relationship：
Wherein,Indicate former antithesis decision variable；Indicate equality constraint
(23) antithesis decision variable；Indicate the antithesis decision variable of inequality constraints (24)；Ω is as a convex set
It is defined as foloows：
Wherein, lower limit u-With upper limit u+Respectively：
ε > > 0 are set as far as possible
Greatly for replacement+∞；VectorAnd other parameters are defined as：
Wherein, E is the unit matrix of n × n.
9. a kind of barrier according to claim 8 hides method with physics limit, which is characterized in that the step S8
It is as follows：
Above-mentioned linear differential inequality (26) is transformed into the linear projection equation with following form：
Wherein,For fromProject to the linear projection operator of set omega；MatrixU andWith it is aforementioned fixed
Justice is consistent.
10. a kind of barrier according to claim 9 hides method with physics limit, which is characterized in that the step
S9 is as follows：
Above-mentioned linear projection equation (36) is substituted into following primal-dual neural network solver to resolve：
The data obtained output will be resolved, you can complete goal systems and hide to barrier and physics limit.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201810412706.7A CN108772835B (en) | 2018-05-03 | 2018-05-03 | Obstacle and physical limit avoiding method |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201810412706.7A CN108772835B (en) | 2018-05-03 | 2018-05-03 | Obstacle and physical limit avoiding method |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN108772835A true CN108772835A (en) | 2018-11-09 |
| CN108772835B CN108772835B (en) | 2020-09-22 |
Family
ID=64026815
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201810412706.7A Active CN108772835B (en) | 2018-05-03 | 2018-05-03 | Obstacle and physical limit avoiding method |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN108772835B (en) |
Cited By (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109556609A (en) * | 2018-11-15 | 2019-04-02 | 武汉南华工业设备工程股份有限公司 | A kind of collision prevention method and device based on artificial intelligence |
| CN111309002A (en) * | 2019-11-26 | 2020-06-19 | 华南理工大学 | Wheel type mobile robot obstacle avoidance method and system based on vector |
| CN113334379A (en) * | 2021-05-30 | 2021-09-03 | 南京理工大学 | Master-slave following and collision avoidance method based on virtual force |
| WO2023024279A1 (en) * | 2021-08-24 | 2023-03-02 | 深圳市优必选科技股份有限公司 | Dynamic obstacle avoidance method and apparatus for robot, and robot |
| CN116719417A (en) * | 2023-08-07 | 2023-09-08 | 海马云（天津）信息技术有限公司 | Motion constraint method and device for virtual digital person, electronic equipment and storage medium |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| KR20150127899A (en) * | 2014-05-07 | 2015-11-18 | 서울대학교산학협력단 | Cooperative Grasping Control and Obstacle Avoidance of Multiple Mobile Manipulator |
| CN104760041B (en) * | 2015-03-19 | 2016-08-03 | 中山大学 | A kind of Obstacle avoidance motion planning method based on impact degree |
| CN107066698A (en) * | 2017-03-18 | 2017-08-18 | 华南理工大学 | Repetitive motion planning method for redundant manipulator based on New Type of Numerical solver |
| CN107490965A (en) * | 2017-08-21 | 2017-12-19 | 西北工业大学 | A kind of multiple constraint method for planning track of the free floating devices arm in space |
| CN107957684A (en) * | 2017-11-17 | 2018-04-24 | 华南理工大学 | A kind of robot three-dimensional based on pseudo-velocity vector field is without touching method for planning track |
| CN107966907A (en) * | 2017-11-30 | 2018-04-27 | 华南理工大学 | A kind of Obstacle avoidance applied to redundancy mechanical arm solves method |
-
2018
- 2018-05-03 CN CN201810412706.7A patent/CN108772835B/en active Active
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| KR20150127899A (en) * | 2014-05-07 | 2015-11-18 | 서울대학교산학협력단 | Cooperative Grasping Control and Obstacle Avoidance of Multiple Mobile Manipulator |
| CN104760041B (en) * | 2015-03-19 | 2016-08-03 | 中山大学 | A kind of Obstacle avoidance motion planning method based on impact degree |
| CN107066698A (en) * | 2017-03-18 | 2017-08-18 | 华南理工大学 | Repetitive motion planning method for redundant manipulator based on New Type of Numerical solver |
| CN107490965A (en) * | 2017-08-21 | 2017-12-19 | 西北工业大学 | A kind of multiple constraint method for planning track of the free floating devices arm in space |
| CN107957684A (en) * | 2017-11-17 | 2018-04-24 | 华南理工大学 | A kind of robot three-dimensional based on pseudo-velocity vector field is without touching method for planning track |
| CN107966907A (en) * | 2017-11-30 | 2018-04-27 | 华南理工大学 | A kind of Obstacle avoidance applied to redundancy mechanical arm solves method |
Cited By (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109556609A (en) * | 2018-11-15 | 2019-04-02 | 武汉南华工业设备工程股份有限公司 | A kind of collision prevention method and device based on artificial intelligence |
| CN109556609B (en) * | 2018-11-15 | 2020-11-17 | 武汉南华工业设备工程股份有限公司 | Artificial intelligence-based collision avoidance method and device |
| CN111309002A (en) * | 2019-11-26 | 2020-06-19 | 华南理工大学 | Wheel type mobile robot obstacle avoidance method and system based on vector |
| CN113334379A (en) * | 2021-05-30 | 2021-09-03 | 南京理工大学 | Master-slave following and collision avoidance method based on virtual force |
| WO2023024279A1 (en) * | 2021-08-24 | 2023-03-02 | 深圳市优必选科技股份有限公司 | Dynamic obstacle avoidance method and apparatus for robot, and robot |
| CN116719417A (en) * | 2023-08-07 | 2023-09-08 | 海马云（天津）信息技术有限公司 | Motion constraint method and device for virtual digital person, electronic equipment and storage medium |
| CN116719417B (en) * | 2023-08-07 | 2024-01-26 | 海马云（天津）信息技术有限公司 | Motion constraint method and device for virtual digital person, electronic equipment and storage medium |
Also Published As
| Publication number | Publication date |
|---|---|
| CN108772835B (en) | 2020-09-22 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN108772835A (en) | A kind of barrier hides method with physics limit | |
| CN108838991A (en) | It is a kind of from main classes people tow-armed robot and its to the tracking operating system of moving target | |
| Liu et al. | Obstacle avoidance path planning of space manipulator based on improved artificial potential field method | |
| CN108068113A (en) | 7-DOF humanoid arm flying object operation minimum acceleration trajectory optimization | |
| Huber et al. | Avoiding dense and dynamic obstacles in enclosed spaces: Application to moving in crowds | |
| Zhang et al. | Physical-limits-constrained minimum velocity norm coordinating scheme for wheeled mobile redundant manipulators | |
| Yang et al. | Humanoid motion planning of robotic arm based on human arm action feature and reinforcement learning | |
| Kim et al. | Quaternion based three-dimensional impact angle control guidance law | |
| Jakobović et al. | Forward kinematics of a Stewart platform mechanism | |
| CN106863297B (en) | A kind of accurate approach method of space rope system robot vision | |
| Kabutan et al. | Motion planning by T‐RRT with potential function for vertical articulated robots | |
| Borysov et al. | Parameters for Mobile Robot Kinematic Model Development Determination | |
| Chen et al. | Quadratic programming-based approach for autonomous vehicle path planning in space | |
| Liu et al. | Teleoperation for space manipulator based on complex virtual fixtures | |
| Weibel et al. | Averaging and energy methods for robust open-loop control of mechanical systems | |
| Wang et al. | Dual-equal-collision-probability-curve method for spacecraft safe proximity maneuvers in presence of complex shape | |
| Linquan et al. | Path planning algorithm for mobile robot obstacle avoidance adopting Bezier curve based on genetic algorithm | |
| He et al. | A VR-based complex equipment maintenance training system | |
| Le et al. | Multi-Robot Motion Planning with Dynamics Guided by Multi-Agent Search. | |
| Ravindran et al. | Autonomous multi-robot platoon monitoring | |
| Chen et al. | APF-S2T: Steering to Target Redirection Walking Based on Artificial Potential Fields | |
| Dubanov | Trajectory Modeling in a Pursuit Problem with Curvature Restrictions | |
| Liu et al. | A trajectory generator for a mobile robot in 3D pathplanning | |
| Du et al. | Active Collision Avoidance for Human-Manipulator Safety | |
| Ciarambino et al. | Development validation and test of optical based algorithms for autonomous planetary landing |
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 |