CN111633646B - Robot motion planning method based on DMPs and modified obstacle avoidance algorithm - Google Patents

Robot motion planning method based on DMPs and modified obstacle avoidance algorithm Download PDF

Info

Publication number
CN111633646B
CN111633646B CN202010443890.9A CN202010443890A CN111633646B CN 111633646 B CN111633646 B CN 111633646B CN 202010443890 A CN202010443890 A CN 202010443890A CN 111633646 B CN111633646 B CN 111633646B
Authority
CN
China
Prior art keywords
obstacle avoidance
dmps
obstacle
track
dynamic
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.)
Active
Application number
CN202010443890.9A
Other languages
Chinese (zh)
Other versions
CN111633646A (en
Inventor
翟弟华
夏志强
吴浩存
夏元清
张金会
戴荔
邹伟东
闫莉萍
孙中奇
崔冰
刘坤
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Beijing Institute of Technology BIT
Original Assignee
Beijing Institute of Technology BIT
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Beijing Institute of Technology BIT filed Critical Beijing Institute of Technology BIT
Priority to CN202010443890.9A priority Critical patent/CN111633646B/en
Publication of CN111633646A publication Critical patent/CN111633646A/en
Application granted granted Critical
Publication of CN111633646B publication Critical patent/CN111633646B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

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
    • 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 invention discloses a robot motion planning method based on DMPs and a modified obstacle avoidance algorithm, which can improve the obstacle avoidance effect, thereby assisting the DMPs to better complete a motion planning task. The technical scheme of the invention comprises the following steps: and planning an obstacle-free path from the initial position to the target position for the mechanical arm of the robot as an expected track by adopting a dynamic motion primitive DMPs method. And adding the corrected obstacle avoidance algorithm as a coupling term into a DMPs second-order system for generating an obstacle avoidance track, and using the generated obstacle avoidance track as a motion track of the robot. Preferably, the modified obstacle avoidance algorithm comprises one or more of a combination of modified steering behavior, dynamic approximation and dynamic obstacle avoidance.

Description

Robot motion planning method based on DMPs and modified obstacle avoidance algorithm
Technical Field
The invention belongs to the field of mechanical arm motion planning, and particularly relates to a mechanical arm motion planning method based on DMPs and a modified obstacle avoidance algorithm.
Background
The mechanical arm is a key component of an industrial automation, health medical treatment and aerospace unmanned system, and is one of the most active research directions in the fields of intelligent robots, artificial intelligence and the like. The mechanical arm needs to learn to simulate the grabbing and space movement functions of hands and arms, and has autonomous capabilities of task analysis, environment perception, movement planning, trajectory tracking, automatic obstacle avoidance and the like. The research relates to the scientific and technical fields of machinery, kinematics and dynamics, electronics, computers, information processing, control, artificial intelligence and the like.
DMPs are defined as the action units of a stable nonlinear attractor system. DMPs are a motion control strategy for coding teaching tracks, and develop and apply a novel practical technology developed by a special nonlinear effect. DMPs learn the track attribute through forcing terms, and the most prominent feature of the DMPs is that the DMPs can be used for reproducing teaching tracks and can be popularized to different starting positions and target positions.
Planning using DMPs requires that the trajectory generated in the obstacle environment be as similar as possible to the expected trajectory, which highlights the learning ability of DMPs to the trajectory throughout the planning process. At present, the problem to be solved at present is to use DMPs and a traditional steering behavior method to combine to perform motion planning in a severe environment, and the obstacle avoidance effect is still a big pain point, so that an obstacle avoidance algorithm is improved, and the DMPs are assisted to better complete a motion planning task.
Disclosure of Invention
In view of this, the invention provides a robot motion planning method based on DMPs and a modified obstacle avoidance algorithm, which can improve the obstacle avoidance effect, thereby assisting the DMPs to better complete the motion planning task.
In order to achieve the purpose, the technical scheme of the invention comprises the following steps:
and planning an obstacle-free path from the initial position to the target position for the mechanical arm of the robot as an expected track by adopting a dynamic motion primitive DMPs method.
And adding the corrected obstacle avoidance algorithm as a coupling term into a DMPs second-order system for generating an obstacle avoidance track, and using the generated obstacle avoidance track as a motion track of the robot.
Preferably, the modified obstacle avoidance algorithm comprises one or more of a combination of modified steering behavior, dynamic approximation and dynamic obstacle avoidance.
Preferably, the modified obstacle avoidance algorithm is an improved steering behavior method, and the modified obstacle avoidance algorithm is added to a DMPs second-order system as a coupling term to generate an obstacle avoidance trajectory, specifically:
the improved steering behavior method calculates a first component C1：
Figure GDA0002591688630000021
Wherein: gamma is a preset normal number; v represents the velocity of the end effector of the robotic arm; d | ═ X-XOI represents the Euclidean distance between the mechanical arm end effector and the obstacle; x represents the position of the end effector of the robotic arm; xOIndicating the position of the obstacle; r ∈ SO (3) is a round V × (X)O-X) rotation of the axis
Figure GDA0002591688630000022
The rotation matrix of (a); μ and η represent normal number factors that affect distance and angle, respectively; the direction angle theta is formed by V and XO-X jointly determines:
Figure GDA0002591688630000023
using said first composition item C1Is added into a DMPs second-order system as a coupling term.
Preferably, the modified obstacle avoidance algorithm is a combination of an improved steering behavior method and a dynamic approximation method, and the modified obstacle avoidance algorithm is added to a DMPs second-order system as a coupling term to generate an obstacle avoidance trajectory, specifically:
the improved steering behavior method calculates a first component C1：
Figure GDA0002591688630000031
Wherein: gamma is a preset normal number; v represents the velocity of the end effector of the robotic arm; d | ═ X-XOI denotes a robot arm end effector andthe euclidean distance between obstacles; x represents the position of the end effector of the robotic arm; xOIndicating the position of the obstacle; r ∈ SO (3) is a round V × (X)O-X) rotation of the axis
Figure GDA0002591688630000032
The rotation matrix of (a); μ and η represent normal number factors that affect distance and angle, respectively; the direction angle theta is formed by V and XO-X jointly determines:
Figure GDA0002591688630000033
the dynamic approximation method calculates a second composition term C2：
Figure GDA0002591688630000034
Wherein:
Figure GDA0002591688630000035
|Δx|＝|Xd-X|
wherein:
Figure GDA0002591688630000036
representing an angle between a current speed direction and a desired speed direction; | Δ x | represents the euclidean distance between the current trajectory and the desired trajectory; vdRepresenting the velocity of the desired trajectory obtained with DMPs in the clear; xdIndicating the location of the desired trajectory obtained using DMPs without obstruction; μ' and ρ represent normal number factors that affect distance and angle, respectively.
Combining the first component C1With said second constituent C2Added as a coupling term C to a DMPs second-order system:
Figure GDA0002591688630000037
preferably, the modified obstacle avoidance algorithm is a combination of an improved steering behavior method, a dynamic approximation method and a dynamic obstacle avoidance method, and the modified obstacle avoidance algorithm is added to the DMPs second-order system as a coupling term to generate an obstacle avoidance trajectory, specifically:
the improved steering behavior method calculates a first component C1：
Figure GDA0002591688630000041
Wherein: gamma is a preset normal number; v represents the velocity of the end effector of the robotic arm; d | ═ X-XOI represents the Euclidean distance between the mechanical arm end effector and the obstacle; x represents the position of the end effector of the robotic arm; xOIndicating the position of the obstacle; r ∈ SO (3) is a round V × (X)O-X) rotation of the axis
Figure GDA0002591688630000042
The rotation matrix of (a); μ and η represent normal number factors that affect distance and angle, respectively; the direction angle theta is formed by V and XO-X jointly determines:
Figure GDA0002591688630000043
the dynamic approximation method calculates a second composition term C2：
Figure GDA0002591688630000044
Wherein:
Figure GDA0002591688630000045
|Δx|＝|Xd-X|
wherein:
Figure GDA0002591688630000046
representing an angle between a current speed direction and a desired speed direction; | Δ x | represents the euclidean distance between the current trajectory and the desired trajectory; vdRepresenting the velocity of the desired trajectory obtained with DMPs in the clear; xdIndicating the location of the desired trajectory obtained using DMPs without obstruction; μ' and ρ represent normal number factors that affect distance and angle, respectively.
Figure GDA0002591688630000047
The dynamic obstacle avoidance method comprises the following steps:
Vrelative＝V-Vo；
wherein, VrelativeRepresenting the relative velocity, V, between the end-effector of the robot arm and the obstacleoIndicating the speed of the obstacle by VrelativeIn place of C1-C2V in the formula, the coupling term C is obtained as:
Figure GDA0002591688630000051
has the advantages that:
the robot motion planning method based on the DMPs and the modified obstacle avoidance algorithm, provided by the invention, comprises the steps of firstly planning an obstacle-free path from an initial position to a target position as an expected path for a mechanical arm by utilizing the track learning capacity of the DMPs; then adding the corrected obstacle avoidance algorithm as a coupling term into a DMPs second-order system for generating an obstacle avoidance track; finally, aiming at the motion planning method based on the DMPs and the modified obstacle avoidance algorithm, the stability of the motion planning method is proved in a theoretical level, and the simulation under different conditions and the Baxter mechanical arm real verification prove that the method can provide a reliable motion planning scheme. When the initial position or the target position of the track changes, a new track can be rapidly planned by the robot motion planning method based on the DMPs and the modified obstacle avoidance algorithm. When the obstacle is close to the initial position of the track, the damage to the expected track is large, and the direction of the expected track is changed violently, the robot motion planning method based on the DMPs and the modified obstacle avoidance algorithm can eliminate jitter, improve the obstacle avoidance capability of the system, maintain the similarity with the expected track as much as possible, and reduce the loss of free space. When dynamic obstacles exist in the environment, the robot motion planning method based on the DMPs and the modified obstacle avoidance algorithm can plan a high-quality track in real time.
Drawings
FIG. 1 is a schematic diagram of the operation of DMPs;
FIG. 2 is a schematic diagram of the generation of high-dimensional trajectories for DMPs;
FIG. 3 is a schematic diagram of a conventional steering behavior method;
fig. 4 is an obstacle avoidance trajectory diagram obtained by a conventional steering behavior method;
FIG. 5 is a diagram of an obstacle avoidance trajectory obtained by an improved steering behavior method;
FIG. 6 is a schematic diagram of the operation of dynamic approximation;
FIG. 7 is a diagram of an obstacle avoidance trajectory obtained by dynamic approximation;
FIG. 8 is a diagram of a dynamic obstacle avoidance process;
fig. 9 is an obstacle avoidance trajectory diagram obtained by the obstacle at different positions;
fig. 10 is an obstacle avoidance trajectory diagram of an obstacle in different postures;
fig. 11 is an obstacle avoidance trajectory diagram obtained after the initial position and the target position are modified.
Detailed Description
The invention is described in detail below by way of example with reference to the accompanying drawings.
The modified obstacle avoidance method is a novel practical technology developed based on a traditional steering behavior method by considering an obstacle avoidance effect and DMPs characteristics. The most prominent characteristic is that the method can eliminate jitter, improve the obstacle avoidance capability of the system, simultaneously can keep the attribute of the expected track as much as possible, reduce the loss of free space, and adapt to the dynamic change of the obstacle. The significance of combining the two is that motion planning under the environment of the obstacle can be realized, the DMPs are responsible for learning the teaching track and generating the expected track, and the obstacle avoidance algorithm is modified to assist the DMPs in realizing the obstacle avoidance effect.
The invention provides a robot motion planning method based on DMPs and a modified obstacle avoidance algorithm, which comprises the following steps:
the first step is as follows: the trajectory learning capabilities using DMPs are described in FIG. 1 as follows:
Figure GDA0002591688630000061
where τ denotes a time scale factor, x denotes current location information of the system, g denotes target location information of the system, v and
Figure GDA0002591688630000062
representing the velocity and acceleration of the system, respectively, α and β are the normal numbers representing the elasticity and damping, respectively, f(s) is a forcing term consisting of weighted N radial basis functions, which can be defined as:
Figure GDA0002591688630000063
wherein
Figure GDA0002591688630000071
Representing the radial basis function, ωiRepresenting the weight of the radial basis function, here the radial basis function
Figure GDA0002591688630000072
Can be defined as:
Figure GDA0002591688630000073
wherein h isiAnd ciRespectively represent the ith e [1, N ∈]The width and center of each radial basis function, the specific result can be found according to the following equation:
Figure GDA0002591688630000074
hi＝(ci+1-ci)-2 hN＝hN-1 (5)
where λ is a predefined positive constant. The weights ω applied to the basis functions may be selected using optimization techniques such as linear regressioniSo that the forcing term matches the desired trajectory. When changing the starting position or the ending position, ω is obtainediHas certain generalization capability to generate corresponding tracks.
The speed of the system motion can be adjusted by a time-related scaling factor τ, which is usually equal to 1, and this factor directly determines the duration of the canonical system. The transient behavior of the phase variable s of the canonical system can be defined as:
Figure GDA0002591688630000075
next, an unobstructed path from the start position to the target position is planned for the robotic arm as a desired trajectory, as shown in fig. 2, and described below:
Figure GDA0002591688630000076
wherein X ∈ Rd,V∈Rd,G∈RdAre vectors, X, representing the current position, the current velocity and the target position, respectivelyinitIs a vector representing the initial position, d ∈ N*Can be any positive integer, and the diagonal matrix A of dimension d x d is diag [ alpha ]1,α2,…,αd]And B ═ diag [ beta ]1,β2,…,βd]Respectively representing an elastic term and a damping term. F is belonged to RdIs a vector representing the forcing term, the jth element in F can be represented as:
Figure GDA0002591688630000077
the motion in each dimension is independent and shares the same specification system with each other. Therefore, in the field of robot motion planning, the DMPs can learn the motion trail of the joint space of the mechanical arm and the motion trail of the end effector of the mechanical arm in the Cartesian space, and the invention mainly researches the motion trail of the end effector of the mechanical arm in the Cartesian space RdThe motion planning problem of (1), d-2 for planar scenes and d-3 for spatial scenes, considers the end effector of the mechanical arm as an ideal point and defines the obstacle in cartesian space Rd。
The second step is that: the modified obstacle avoidance algorithm is added to a DMPs second-order system as a coupling term for generating an obstacle avoidance track, and mainly comprises the design of three aspects of an improved steering behavior method, a dynamic approximation method and a dynamic obstacle avoidance method:
1. improved steering behavior
The traditional steering behavior is rotation
Figure GDA0002591688630000081
Taking the current velocity vector V and the vector X of the mechanical arm end effector pointing to the obstacle as the referenceOThe normal vector of the plane formed by X is the rotation axis, as shown in FIG. 3(a), the current velocity vector V and the vector X of the robot arm end effector pointing to the obstacleOThe angle θ between X can be defined as:
Figure GDA0002591688630000082
wherein XORepresenting obstacle position,. representing the inner product of the vectors, |, representing the modulo length of the vectors, as shown in fig. 3(b), steering speed
Figure GDA0002591688630000083
Is dependent on θ, and
Figure GDA0002591688630000084
can be defined as:
Figure GDA0002591688630000085
where γ and η are normal numbers, therefore, the obstacle avoidance coupling term that can change the velocity direction can be defined as:
Figure GDA0002591688630000086
wherein R ∈ SO (3) is a round V × (X)O-X) rotation of the axis
Figure GDA0002591688630000087
X represents the outer product between vectors. When the method is adopted to avoid the obstacle, the following two obvious problems can occur: when the obstacle is close to the initial position, the damage to the track is large, and the direction change of the expected track is severe, the problems of jitter, incapability of avoiding the obstacle and the like occur, as shown in fig. 4.
Therefore, the patent is improved on the basis of the method. In the initial stage, the distance between the end effector of the mechanical arm and the obstacle will be an important factor. Considering that the distance between the end effector of the mechanical arm and the obstacle is integrated into the steering behavior method, the distance | d | ═ X-X between the end effector of the mechanical arm and the obstacle isOL is used as an index for measuring the degree of rotation, and the magnitude of the repulsive force generated thereby is:
Figure GDA0002591688630000091
wherein, XOIndicating the position of the obstacle and μ and η representing normal number factors affecting distance and angle, respectively. After adding (12) to (7), the state transition of the system can be described in the following form:
Figure GDA0002591688630000092
at this time, the obtained obstacle avoidance track is shown in fig. 5, and it can be found that: under the action of the new obstacle avoidance coupling term C, the jitter can be eliminated, the system can effectively avoid the obstacle, the similarity with the expected track is kept as much as possible, and the loss of the free space is reduced.
2. Dynamic approximation method
A large number of experiments show that the obstacle avoidance track is greatly influenced by factors such as the position and the posture of an obstacle. Therefore, consider a reaction force C that increases the repulsive force on the obstacle avoidance coupling term2The influence of the obstacle avoidance coupling item on the whole system is balanced.
Taking into account the angle between the current speed direction and the desired speed direction in the course of the dynamic adjustment
Figure GDA0002591688630000093
As an influencing factor:
Figure GDA0002591688630000094
wherein, VdRepresenting the desired trajectory speed obtained with DMPs in the clear, with the goal of minimizing the speed as much as possible
Figure GDA0002591688630000095
To ensure that the generated trajectory is highly similar to the desired trajectory; but also taking into account the euclidean distance | Δ x | between the current trajectory and the desired trajectory as an obstacle avoidance term:
|Δx|＝|Xd-X| (15)
wherein, XdRepresenting the position of the desired trajectory obtained using DMPs without obstruction, the greater | Δ x | the greater the value of C2The larger, to ensure as little loss of free space as possible; meanwhile, the influence of the distance | d | between the mechanical arm end effector and the obstacle on the track is also considered, and the distance | d | is reduced to enable C to be smaller2The impact on the system is reduced. Dynamic approximationThe working principle of (1) is shown in the attached figure 6, C2Can be defined as:
Figure GDA0002591688630000101
at this time, the obstacle avoidance coupling term of the system is as follows:
Figure GDA0002591688630000102
where μ' and ρ represent normal number factors that affect distance and angle, respectively. The obtained obstacle avoidance track can be found in the attached figure 7: the system has less free space loss and the similarity between the generated track and the expected track is improved.
3. Dynamic obstacle avoidance method
Dynamic obstacles are very common in practical application, and particularly in the process of double-arm cooperation, the situation that the position of the obstacle is dynamically changed is inevitably met, and the moving speed V of the obstacle needs to be considered at this timeoAnd dynamic variation of the distance | d | between the end effector of the mechanical arm and the obstacle. The resulting relative velocities were:
Vrelative＝V-Vo (18)
at this time, the obstacle avoidance coupling term of the system is as follows:
Figure GDA0002591688630000103
note that | d | herein is to be calculated in real time according to the states of the robot arm end effector and the obstacle. The dynamic obstacle avoidance simulation process is shown in fig. 8(a), (b) and (c), and can be found out that: the system can effectively realize obstacle avoidance, and has higher similarity with an expected track and less free space loss.
Demonstration of stability
Whether the motion can be ensured to be converged to the target state after the obstacle avoidance coupling term is added in the DMPs system draws much attention, so that the stability of the system is proved by utilizing the Lyapunov criterion to ensure that the motion can be converged to the target state aiming at any initial state. In an environment with a static obstacle, the system equation after adding the obstacle avoidance term can be expressed as follows:
Figure GDA0002591688630000111
let (G,0,0) be
Figure GDA0002591688630000112
For any target state(s) of (c), constructing a Lyapunov function M > 0, it is only necessary to prove that for any state not equal to (G,0,0)
Figure GDA0002591688630000113
Satisfy the result after M derivation
Figure GDA0002591688630000114
I.e. it can be said that any initial state will eventually converge to the target state. The expression for M can be described as:
Figure GDA0002591688630000115
obviously, when G ═ X and V ═ 0, M ═ 0. Then, taking the derivative of M, the following expression can be obtained:
Figure GDA0002591688630000116
since (6) is an ordinary differential equation, with t → ∞, s → 0, part2 → 0, and since (8) s is known to be a factor of F, part3 → 0. Also because R ∈ SO (3) represents a rotation matrix rotated by 90 DEG, VTRV is 0. And VTRV is a factor of part4, so part4 is 0. Therefore, the temperature of the molten metal is controlled,
Figure GDA0002591688630000117
can be simplified to the following form:
Figure GDA0002591688630000121
when k is1If < 0, B > A | G-X can be obtainedinit|ε1+Aε2. And k is2Is a positive number, and k2The value range is as follows:
Figure GDA0002591688630000122
wherein s isinitDenotes the initial value of s, FinitDenotes the initial value of F. k is a radical of2And the system has an upper bound and can be converged to 0 along with t → ∞ monotony, so that the system complies with the Lyapunov criterion and meets the stability requirement.
In solving for
Figure GDA0002591688630000123
When known, it is
Figure GDA0002591688630000124
At this time, k exists1VTAnd V is 0. And because of k1If the value is less than 0, V can be obtained as 0. At this time, the solution
Figure GDA0002591688630000125
Equivalent to solving for k { (X, V) | V ═ 0 }. According to the Lassel invariance theorem, assuming that V (t) is also a solution belonging to κ, for a trace to be included in κ, V (t) of the trace must be constantly equal to 0:
V(t)≡0 (24)
when v (t) is constant and equal to 0,
Figure GDA0002591688630000126
will also be equal to 0:
Figure GDA0002591688630000127
therefore, any initial state will eventually converge to the target state
Figure GDA0002591688630000128
In an environment containing dynamic obstacles, referring to the above-mentioned certification process, it is only necessary to make V ═ VrelativeAt this time, VrelativeC is still satisfied when C is 0, and it can be verified that any initial state will eventually converge to the target state
Figure GDA0002591688630000129
Experimental Environment
To verify the effectiveness of the proposed method, experiments were performed on a Baxter robotic arm. The mechanical arm is controlled by an ROS frame and a MoveIT interface, and a track is planned and executed based on DMPs and a modified obstacle avoidance algorithm in a feasible region of the mechanical arm.
The invention mainly aims at the motion planning in severe environment, because the obstacle avoidance effect is influenced by the change rate of the track direction, the provided learning track ensures different curvatures in different stages, and the learning track provided in the experiment is
Figure GDA0002591688630000131
And
Figure GDA0002591688630000132
where t ∈ (0, π). In order to verify the reliability of the obstacle avoidance effect of the method in a severe environment, a series of experiments are required to be carried out on obstacles in different states. In order to verify that the method still has the track learning capability while realizing obstacle avoidance in a severe environment, a series of experiments need to be carried out after the initial position and the target position are changed. In the experimental process, the obstacles are placed at different positions and different postures to observe the experimental effect, and the experimental effect is observed after the initial position and the target position are changed independently or simultaneously.
Results of the experiment
FIGS. 9 and 10 show the obstacle in different statesFig. 11 is an obstacle avoidance trajectory diagram obtained after changing the initial position and the target position, in which a dotted line represents an expected trajectory planned without an obstacle based on the DMPs, an ellipse-shaped point set represents an obstacle, and a dot-dash line represents an obstacle avoidance trajectory obtained by a robot motion planning method based on the DMPs and a modified obstacle avoidance algorithm. Fig. 9 is the experimental results of the obstacle in different positions: fig. 9(a) is an obstacle avoidance trajectory diagram of an obstacle at the initial stage of the trajectory, where the direction of the trajectory changes more sharply; fig. 9(b) is an obstacle avoidance trajectory diagram of an obstacle at an intermediate stage of a trajectory, where a change in the direction of the trajectory is relatively gradual; fig. 9(c) is an obstacle avoidance trajectory diagram of an obstacle at the end stage of the trajectory, where the direction of the trajectory changes in the conventional case. Fig. 10 shows the experimental results of placing an obstacle in the initial stage of the trajectory, with the obstacle in different poses: FIG. 10(a) shows the obstacle rotating
Figure GDA0002591688630000133
Obtaining an obstacle avoidance track graph; FIG. 10(b) shows the obstacle rotating
Figure GDA0002591688630000134
Obtaining an obstacle avoidance track graph; FIG. 10(c) shows the obstacle rotating
Figure GDA0002591688630000135
And obtaining the obstacle avoidance track map. FIG. 11 is the experimental results after changing the starting and target positions: FIG. 11(a) shows a modification of the starting position to Xinit＝[0.15,-0.15]The obtained obstacle avoidance trajectory graph is shown in fig. 11(b) in which the starting position is modified to G [ -1.1,0.2 [ ]]The obtained obstacle avoidance track graph is shown in the figure 11(c), the starting position and the target position are simultaneously modified into Xinit＝[0.15,-0.15]And G [ -1.1,0.2 [ ]]And obtaining the obstacle avoidance track map.
The results of fig. 9 and fig. 10 show that the robot motion planning method based on DMPs and the modified obstacle avoidance algorithm has good adaptability to environmental changes, and can successfully complete the motion planning task while ensuring the obstacle avoidance effect in both severe environments and common environments. The result of fig. 11 shows that the robot motion planning method based on DMPs and the modified obstacle avoidance algorithm still has better track learning capability.
In summary, the above description is only a preferred embodiment of the present invention, and is not intended to limit the scope of the present invention. Any modification, equivalent replacement, or improvement made within the spirit and principle of the present invention should be included in the protection scope of the present invention.

Claims (1)

1. The robot motion planning method based on DMPs and the modified obstacle avoidance algorithm is characterized by comprising the following steps of:
planning an obstacle-free path from an initial position to a target position for a mechanical arm of the robot by adopting a dynamic motion primitive DMPs method to serve as an expected track;
adding the corrected obstacle avoidance algorithm as a coupling term into a DMPs second-order system for generating an obstacle avoidance track, and using the generated obstacle avoidance track as a motion track of the robot;
the modified obstacle avoidance algorithm comprises the following steps: a an improved steering behavior method is provided,
or B a combination of modified steering behavior and dynamic approximation,
or C, combining an improved steering behavior method, a dynamic approximation method and a dynamic obstacle avoidance method;
a: the modified obstacle avoidance algorithm is an improved steering behavior method, is used as a coupling term and is added to a DMPs second-order system to generate an obstacle avoidance track, and specifically comprises the following steps:
the improved steering behavior method calculates a first component C1：
Figure FDA0003138761180000011
Wherein: gamma is a preset normal number; v represents the velocity of the end effector of the robotic arm; d | ═ X-X0I represents the Euclidean distance between the mechanical arm end effector and the obstacle; x denotes a robot armA position of the end effector; xOIndicating the position of the obstacle; r ∈ SO (3) is a round V × (X)O-X) rotation of the axis
Figure FDA0003138761180000012
The rotation matrix of (a); μ and η represent normal number factors that affect distance and angle, respectively; the direction angle theta is formed by V and XO-X jointly determines:
Figure FDA0003138761180000013
using said first composition item C1Adding the coupling term into a DMPs second-order system;
b: the modified obstacle avoidance algorithm is a combination of an improved steering behavior method and a dynamic approximation method, is added to a DMPs second-order system as a coupling term to generate an obstacle avoidance track, and specifically comprises the following steps:
the improved steering behavior method calculates a first component C1：
Figure FDA0003138761180000021
Wherein: gamma is a preset normal number; v represents the velocity of the end effector of the robotic arm; d | ═ X-XOI represents the Euclidean distance between the mechanical arm end effector and the obstacle; x represents the position of the end effector of the robotic arm; xOIndicating the position of the obstacle; r ∈ SO (3) is a round V × (X)O-X) rotation of the axis
Figure FDA0003138761180000022
The rotation matrix of (a); μ and η represent normal number factors that affect distance and angle, respectively; the direction angle theta is formed by V and XO-X jointly determines:
Figure FDA0003138761180000023
the dynamic approximation method calculates a second composition term C2：
Figure FDA0003138761180000024
Wherein:
Figure FDA0003138761180000025
|Δx|＝|Xd-X|
wherein:
Figure FDA0003138761180000026
representing an angle between a current speed direction and a desired speed direction; | Δ x | represents the euclidean distance between the current trajectory and the desired trajectory; vdRepresenting the velocity of the desired trajectory obtained with DMPs in the clear; xdIndicating the location of the desired trajectory obtained using DMPs without obstruction; μ' and ρ represent normal number factors that affect distance and angle, respectively;
combining the first component C1With said second constituent C2Added as a coupling term C to a DMPs second-order system:
Figure FDA0003138761180000027
c: the modified obstacle avoidance algorithm is a combination of an improved steering behavior method, a dynamic approximation method and a dynamic obstacle avoidance method, and is added to a DMPs second-order system as a coupling term to generate an obstacle avoidance track, and the modified obstacle avoidance algorithm specifically comprises the following steps:
the improved steering behavior method calculates a first component C1：
Figure FDA0003138761180000031
Wherein: gamma is a preset normal number; v represents the velocity of the end effector of the robotic arm; d | ═ X-XOI represents the Euclidean distance between the mechanical arm end effector and the obstacle; x represents the position of the end effector of the robotic arm; xOIndicating the position of the obstacle; r ∈ SO (3) is a round V × (X)O-X) rotation of the axis
Figure FDA0003138761180000032
The rotation matrix of (a); μ and η represent normal number factors that affect distance and angle, respectively; the direction angle theta is formed by V and XO-X jointly determines:
Figure FDA0003138761180000033
the dynamic approximation method calculates a second composition term C2：
Figure FDA0003138761180000034
Wherein:
Figure FDA0003138761180000035
|Δx|＝|Xd-X|
wherein:
Figure FDA0003138761180000036
representing an angle between a current speed direction and a desired speed direction; | Δ x | represents the euclidean distance between the current trajectory and the desired trajectory; vdRepresenting the velocity of the desired trajectory obtained with DMPs in the clear; xdIndicating the location of the desired trajectory obtained using DMPs without obstruction; μ' and ρ denote normality affecting distance and angle, respectivelyA number factor;
Figure FDA0003138761180000037
the dynamic obstacle avoidance method comprises the following steps:
Vrelative＝V-Vo；
wherein, VrelativeRepresenting the relative velocity, V, between the end-effector of the robot arm and the obstacleoIndicating the speed of the obstacle by VrelativeIn place of C1-C2V in the formula, the coupling term C is obtained as:
Figure FDA0003138761180000041
CN202010443890.9A 2020-05-22 2020-05-22 Robot motion planning method based on DMPs and modified obstacle avoidance algorithm Active CN111633646B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010443890.9A CN111633646B (en) 2020-05-22 2020-05-22 Robot motion planning method based on DMPs and modified obstacle avoidance algorithm

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010443890.9A CN111633646B (en) 2020-05-22 2020-05-22 Robot motion planning method based on DMPs and modified obstacle avoidance algorithm

Publications (2)

Publication Number Publication Date
CN111633646A CN111633646A (en) 2020-09-08
CN111633646B true CN111633646B (en) 2021-08-27

Family

ID=72324814

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010443890.9A Active CN111633646B (en) 2020-05-22 2020-05-22 Robot motion planning method based on DMPs and modified obstacle avoidance algorithm

Country Status (1)

Country Link
CN (1) CN111633646B (en)

Families Citing this family (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112828890B (en) * 2021-01-04 2022-08-02 武汉晴川学院 Mechanical arm track planning method and device, electronic equipment and storage medium
CN112975986B (en) * 2021-03-25 2022-06-24 珞石（北京）科技有限公司 Mechanical arm point-to-point trajectory planning method and device based on radial basis function
CN114310918A (en) * 2022-03-14 2022-04-12 珞石（北京）科技有限公司 Mechanical arm track generation and correction method under man-machine cooperation
CN114721412B (en) * 2022-03-16 2023-06-16 北京理工大学 Unmanned aerial vehicle track tracking obstacle avoidance method based on model predictive control
CN115179282A (en) * 2022-07-07 2022-10-14 珠海格力电器股份有限公司 Robot motion track determination method and device and robot
CN115609595B (en) * 2022-12-16 2023-03-07 北京中海兴达建设有限公司 Trajectory planning method, device and equipment of mechanical arm and readable storage medium

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
DE102012013690A1 (en) * 2012-07-07 2014-01-09 Volkswagen Aktiengesellschaft Method for adapting steering wheel of driver assistance system for e.g. passenger car, involves adapting steering-restoring and steering-support forces applied on steering unit by vehicle as function of steering angle, based on detection
CN106182019A (en) * 2016-07-29 2016-12-07 中国科学技术大学 Industrial robot captures the dynamic obstacle avoidance system and method for process
CN106209615A (en) * 2016-07-05 2016-12-07 云南大学 A kind of Dynamic Route Control method calculating forward-path based on SPFA algorithm
CN106444738A (en) * 2016-05-24 2017-02-22 武汉科技大学 Mobile robot path planning method based on dynamic motion primitive learning model
US9855658B2 (en) * 2015-03-19 2018-01-02 Rahul Babu Drone assisted adaptive robot control
US10008122B2 (en) * 2015-09-28 2018-06-26 The Boeing Company Apparatus to generate aircraft intent and related methods
CN108469732A (en) * 2018-03-13 2018-08-31 同济大学 Robot working space's adaptability traveling control system based on DMP and method
CN108762264A (en) * 2018-05-22 2018-11-06 重庆邮电大学 The dynamic obstacle avoidance method of robot based on Artificial Potential Field and rolling window

Family Cites Families (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP5708196B2 (en) * 2011-04-21 2015-04-30 セイコーエプソン株式会社 Collision detection system, robot system, collision detection method and program
US9403275B2 (en) * 2014-10-17 2016-08-02 GM Global Technology Operations LLC Dynamic obstacle avoidance in a robotic system
CN109960880B (en) * 2019-03-26 2023-01-03 上海交通大学 Industrial robot obstacle avoidance path planning method based on machine learning
CN110640736B (en) * 2019-10-23 2023-04-07 南京工业大学 Mechanical arm motion planning method for intelligent manufacturing

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
DE102012013690A1 (en) * 2012-07-07 2014-01-09 Volkswagen Aktiengesellschaft Method for adapting steering wheel of driver assistance system for e.g. passenger car, involves adapting steering-restoring and steering-support forces applied on steering unit by vehicle as function of steering angle, based on detection
US9855658B2 (en) * 2015-03-19 2018-01-02 Rahul Babu Drone assisted adaptive robot control
US10008122B2 (en) * 2015-09-28 2018-06-26 The Boeing Company Apparatus to generate aircraft intent and related methods
CN106444738A (en) * 2016-05-24 2017-02-22 武汉科技大学 Mobile robot path planning method based on dynamic motion primitive learning model
CN106209615A (en) * 2016-07-05 2016-12-07 云南大学 A kind of Dynamic Route Control method calculating forward-path based on SPFA algorithm
CN106182019A (en) * 2016-07-29 2016-12-07 中国科学技术大学 Industrial robot captures the dynamic obstacle avoidance system and method for process
CN108469732A (en) * 2018-03-13 2018-08-31 同济大学 Robot working space's adaptability traveling control system based on DMP and method
CN108762264A (en) * 2018-05-22 2018-11-06 重庆邮电大学 The dynamic obstacle avoidance method of robot based on Artificial Potential Field and rolling window

Also Published As

Publication number Publication date
CN111633646A (en) 2020-09-08

Similar Documents

Publication Publication Date Title
CN111633646B (en) Robot motion planning method based on DMPs and modified obstacle avoidance algorithm
US11845186B2 (en) Inverse kinematics solving method for redundant robot and redundant robot and computer readable storage medium using the same
CN111618858B (en) Manipulator robust tracking control algorithm based on self-adaptive fuzzy sliding mode
Wang et al. A hybrid visual servo controller for robust grasping by wheeled mobile robots
CN109901397B (en) Mechanical arm inverse kinematics method using particle swarm optimization algorithm
Zhang et al. An obstacle avoidance algorithm for robot manipulators based on decision-making force
Luo et al. Real time human motion imitation of anthropomorphic dual arm robot based on Cartesian impedance control
Ewerton et al. Incremental imitation learning of context-dependent motor skills
Nazemizadeh et al. Trajectory planning of mobile robots using indirect solution of optimal control method in generalized point-to-point task
Rahmani et al. A 7-DoF upper limb exoskeleton robot control using a new robust hybrid controller
Jin et al. Finite-time model predictive tracking control of position and orientation for redundant manipulators
Liu et al. Dynamic obstacle avoidance for cable-driven parallel robots with mobile bases via sim-to-real reinforcement learning
Yang et al. Variable impedance skill learning for contact-rich manipulation
Yuan et al. Hierarchical dynamic movement primitive for the smooth movement of robots based on deep reinforcement learning
Zhai et al. A motion planning method for robots based on DMPS and modified obstacle-avoiding algorithm
Jiang et al. Obstacle avoidance of a redundant robot using virtual force field and null space projection
Jiang et al. A new impedance control method using backstepping approach for flexible joint robot manipulators
Buss et al. Advanced telerobotics: Dual-handed and mobile remote manipulation
Yang et al. A neural network approach to real-time motion planning and control of robot manipulators
Dong et al. A dmp-based online adaptive stiffness adjustment method
Fang et al. Learning from wearable-based teleoperation demonstration
Zhou et al. Neural network-based variable stiffness impedance control for internal/external forces tracking of dual-arm manipulators under uncertainties
Bodor et al. Predictive control of robot manipulators with flexible joints
Odagaki et al. Analysis of the dynamics and nonlinear control of under-actuated brachiation robots
Zhu et al. A dynamics formulation of general constrained robots

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