CN111633646A - 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
CN111633646A
CN111633646A CN202010443890.9A CN202010443890A CN111633646A CN 111633646 A CN111633646 A CN 111633646A CN 202010443890 A CN202010443890 A CN 202010443890A CN 111633646 A CN111633646 A CN 111633646A
Authority
CN
China
Prior art keywords
obstacle avoidance
dmps
obstacle
modified
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.)
Granted
Application number
CN202010443890.9A
Other languages
Chinese (zh)
Other versions
CN111633646B (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 a human hand and the arm, 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 trajectories, and develop and apply a novel practical technology developed by using 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 desired trajectory, which highlights the learning ability of DMPs to the trajectory throughout the planning process. At present, the problem to be solved is urgently needed to be solved at present, namely, the motion planning is performed by combining the DMPs with the traditional steering behavior method in the severe environment, and the obstacle avoidance effect is still a big pain point, so that the obstacle avoidance algorithm is improved, and the DMPs are assisted to better complete the 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 starting 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 for generating an obstacle avoidance trajectory, specifically:
the improved steering behavior method calculates a first component C1：
Figure BDA0002505066170000021
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) being a circle around V × (X)O-X) rotation of the axis
Figure BDA0002505066170000022
Mu and η represent normal number factors affecting distance and angle, respectively, and the direction angle theta is defined by V and XO-X jointly determines:
Figure BDA0002505066170000023
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 for generating an obstacle avoidance trajectory, and specifically:
the improved steering behavior method calculates a first component C1：
Figure BDA0002505066170000031
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) being a circle around V × (X)O-X) rotation of the axis
Figure BDA0002505066170000032
Mu and η represent normal number factors affecting distance and angle, respectively, and the direction angle theta is defined by V and XO-X jointly determines:
Figure BDA0002505066170000033
the dynamic approximation method calculates a second composition term C2：
Figure BDA0002505066170000034
Wherein:
Figure BDA0002505066170000035
|Δx|＝|Xd-X|
wherein:
Figure BDA0002505066170000036
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 BDA0002505066170000037
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 BDA0002505066170000041
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) being a circle around V × (X)O-X) rotation of the axis
Figure BDA0002505066170000042
Mu and η represent normal number factors affecting distance and angle, respectively, and the direction angle theta is defined by V and XO-X jointly determines:
Figure BDA0002505066170000043
the dynamic approximation method calculates a second composition term C2：
Figure BDA0002505066170000044
Wherein:
Figure BDA0002505066170000045
|Δx|＝|Xd-X|
wherein:
Figure BDA0002505066170000046
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 BDA0002505066170000047
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 BDA0002505066170000051
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 practical verification of a Baxter mechanical arm prove that the provided 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 BDA0002505066170000061
where τ represents a time scale factor, x represents current location information of the system, g represents target location information of the system, v and
Figure BDA0002505066170000062
representing the velocity and acceleration of the system, α and β are normal numbers representing the elasticity and damping, respectively, and f(s) is a forcing term consisting of weighted N radial basis functions, which can be defined as:
Figure BDA0002505066170000063
wherein
Figure BDA0002505066170000071
Representing the radial basis function, ωiRepresenting the weight of the radial basis function, hereRadial basis function
Figure BDA0002505066170000072
Can be defined as:
Figure BDA0002505066170000073
wherein h isiAnd ciRespectively represent the i ∈ [1, N ]]The width and center of each radial basis function can be found according to the following equation:
Figure BDA0002505066170000074
hi＝(ci+1-ci)-2hN＝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 BDA0002505066170000075
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 BDA0002505066170000076
wherein, X ∈ Rd,V∈Rd,G∈RdRespectively, the current position, the current speed andand vector of target position, XinitIs a vector representing an initial position, d ∈ N*Can be any positive integer, and a diagonal matrix A of d × d dimension is diag [ α ]1,α2,…,αd]And B ═ diag [ β1,β2,…,βd]Respectively representing an elastic term and a damping term F ∈ RdIs a vector representing the forcing term, the jth element in F can be represented as:
Figure BDA0002505066170000077
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 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 robot 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 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 BDA0002505066170000087
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 BDA0002505066170000081
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 BDA0002505066170000082
Is dependent on θ, and
Figure BDA0002505066170000083
can be defined as:
Figure BDA0002505066170000084
where γ and η are normal numbers, therefore, the obstacle avoidance coupling term that can change the velocity direction can be defined as:
Figure BDA0002505066170000085
wherein R ∈ SO (3) is a round V × (X)O-X) rotation of the axis
Figure BDA0002505066170000086
When the method is adopted to avoid the obstacle, the following two obvious problems occur, namely jitter, incapability of avoiding the obstacle and the like 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, 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 BDA0002505066170000091
wherein, XOAfter adding (12) to (7), the state transition of the system can be described in the form:
Figure RE-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 BDA0002505066170000095
As an influencing factor:
Figure BDA0002505066170000093
wherein, VdRepresenting the desired trajectory speed obtained with DMPs in the clear, with the goal of minimizing the speed as much as possible
Figure BDA0002505066170000094
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 end effector of the mechanical arm 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. The operation principle of dynamic approximation is shown in figure 6, C2Can be defined as:
Figure BDA0002505066170000101
at this time, the obstacle avoidance coupling term of the system is as follows:
Figure BDA0002505066170000102
where μ' and ρ represent normal number factors that affect distance and angle, respectively. The obtained obstacle avoidance trajectory is shown in fig. 7, and can be found as follows: 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, particularly in the process of double-arm cooperation, the dynamic change of the position of the obstacle is inevitable, and the dynamic change of the movement speed Vo of the obstacle and the distance | d | between the end effector of the mechanical arm and the obstacle need to be considered at this time. The relative velocities obtained were:
Vrelative＝V-Vo(18)
at this time, the obstacle avoidance coupling term of the system is as follows:
Figure BDA0002505066170000103
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 avoid obstacles, has higher similarity with an expected track and less loss of free space.
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 BDA0002505066170000111
let (G,0,0) be
Figure BDA0002505066170000112
For any target state(s) of (c), constructing a Lyapunov function M > 0, only to prove that for any state not equal to (G,0,0)
Figure BDA0002505066170000113
Satisfy the result after M derivation
Figure BDA0002505066170000114
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 BDA0002505066170000115
obviously, when G ═ X and V ═ 0, M ═ 0. Then, taking the derivative of M, the following expression can be obtained:
Figure BDA0002505066170000116
since (6) is an ordinary differential equation, with t → ∞, s → 0, part2 → 0, and since (8) s is a factor of F, part3 → 0, and since R ∈ SO (3) represents a spinRotation matrix rotated by 90 deg., so VTRV is 0. And VTRV is a factor of part4, so part4 is 0. Therefore, the temperature of the molten steel is controlled,
Figure BDA0002505066170000117
can be simplified to the following form:
Figure BDA0002505066170000121
when k is1If < 0, B > A | G-X can be obtainedinit|1+A2. And k is2Is a positive number, and k2The value range is as follows:
Figure BDA0002505066170000122
wherein s isinitDenotes the initial value of s, FinitDenotes the initial value of F. k is a radical of2The system has an upper bound and converges to 0 along with t → ∞ monotony, so that the system complies with the lyapunov rule and meets the stability requirement.
In solving for
Figure BDA0002505066170000123
When known, it is
Figure BDA0002505066170000124
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 BDA0002505066170000125
Equivalent to solving for k { (X, V) | V ═ 0 }. According to the Lassel invariance theorem, assuming that V (t) is also a solution for κ, in order 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 BDA0002505066170000126
will also be equal to 0:
Figure BDA0002505066170000127
therefore, any initial state will eventually converge to the target state
Figure BDA0002505066170000128
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 BDA0002505066170000129
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 BDA0002505066170000131
And
Figure BDA0002505066170000132
t ∈ (0, pi), in order to verify the reliability of the obstacle avoidance effect of the method in the severe environment, a series of experiments are needed to be carried out on obstacles in different states, in order to verify that the method has the track learning capability while avoiding obstacles in the severe environment, a series of experiments are needed to be carried out after the initial position and the target position are changed, in the experiment process, the obstacles are placed at different positions and different postures to observe the experiment effect, and the experiment effect is observed after the initial position and the target position are changed independently or simultaneously.
Results of the experiment
Fig. 9 and 10 are obstacle avoidance trajectory diagrams obtained by placing an obstacle in different states, fig. 11 is an obstacle avoidance trajectory diagram obtained by changing a start position and a target position, in which a dotted line indicates an expected trajectory planned without an obstacle based on DMPs, an ellipse-shaped point set indicates an obstacle, and a dotted line indicates an obstacle avoidance trajectory obtained by a robot motion planning method based on 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 track diagram of an obstacle at an intermediate stage of a track, where the change of the track direction is relatively gentle; 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 postures: FIG. 10(a) shows the obstacle rotating
Figure BDA0002505066170000133
Obtaining an obstacle avoidance track graph; FIG. 10(b) shows the obstacle rotating
Figure BDA0002505066170000134
Obtaining an obstacle avoidance track graph; FIG. 10(c) shows the obstacle rotating
Figure BDA0002505066170000135
And obtaining the obstacle avoidance track map. FIG. 11 is the experimental results after changing the starting position and the target position: FIG. 11(a) shows a modification of the starting position to Xinit＝[0.15,-0.15]The diagram of the obstacle avoidance trace obtained later 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 protection 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 (5)

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;
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.
2. The method of claim 1, wherein the modified obstacle avoidance algorithm comprises a combination of one or more of modified steering behavior, dynamic approximation, and dynamic obstacle avoidance.
3. The method of claim 1, wherein 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 for generating an obstacle avoidance trajectory, specifically:
the improved steering behavior method calculates a first component C1：
Figure FDA0002505066160000011
Wherein:gamma is a preset normal number; v represents the velocity of the end effector of the robotic arm; d | ═ X-O | represents the euclidean distance between the end effector of the robotic arm and the obstacle; x represents the position of the end effector of the robotic arm; xOIndicating the position of the obstacle, R ∈ SO (3) being a circle around V × (X)O-X) rotation of the axis
Figure FDA0002505066160000012
Mu and η represent normal number factors affecting distance and angle, respectively, and the direction angle theta is defined by V and XO-X jointly determines:
Figure FDA0002505066160000013
using said first composition item C1Is added into a DMPs second-order system as a coupling term.
4. The method of claim 1, wherein 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 as a coupling term to a DMPs second-order system for generating an obstacle avoidance trajectory, specifically:
the improved steering behavior method calculates a first component C1：
Figure FDA0002505066160000021
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) being a circle around V × (X)O-X) rotation of the axis
Figure FDA0002505066160000022
Mu and η represent the normal numbers affecting distance and angle, respectivelyA factor; the direction angle theta is formed by V and XO-X jointly determines:
Figure FDA0002505066160000023
the dynamic approximation method calculates a second composition term C2：
Figure FDA0002505066160000024
Wherein:
Figure FDA0002505066160000025
|Δx|＝|Xd-X|
wherein:
Figure FDA0002505066160000026
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 FDA0002505066160000027
5. the method of claim 1, wherein 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 a DMPs second-order system as a coupling term for generating an obstacle avoidance trajectory, specifically:
the improved steering behavior method calculates a first component C1：
Figure FDA0002505066160000031
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) being a circle around V × (X)O-X) rotation of the axis
Figure FDA0002505066160000032
Mu and η represent normal number factors affecting distance and angle, respectively, and the direction angle theta is defined by V and XO-X jointly determines:
Figure FDA0002505066160000033
the dynamic approximation method calculates a second composition term C2：
Figure FDA0002505066160000034
Wherein:
Figure FDA0002505066160000035
|Δx|＝|Xd-X|
wherein:
Figure FDA0002505066160000036
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; xdIndicate interest in the clearThe location of the desired trajectory obtained with the DMPs; μ' and ρ represent normal number factors that affect distance and angle, respectively;
Figure FDA0002505066160000037
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 FDA0002505066160000041
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 true CN111633646A (en) 2020-09-08
CN111633646B 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)

Cited By (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112828890A (en) * 2021-01-04 2021-05-25 武汉晴川学院 Mechanical arm track planning method and device, electronic equipment and storage medium
CN112975986A (en) * 2021-03-25 2021-06-18 珞石（北京）科技有限公司 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
CN114489055A (en) * 2021-12-31 2022-05-13 中国科学技术大学 Robot multitask movement implementation method, medium and equipment based on sequential logic
CN114721412A (en) * 2022-03-16 2022-07-08 北京理工大学 Unmanned aerial vehicle trajectory 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
CN115609595A (en) * 2022-12-16 2023-01-17 北京中海兴达建设有限公司 Trajectory planning method, device and equipment of mechanical arm and readable storage medium

Citations (12)

* 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
US20140334714A1 (en) * 2011-04-21 2014-11-13 Seiko Epson Corporation Collision detection system, robotic system, collision detection method and program
CN105710888A (en) * 2014-10-17 2016-06-29 通用汽车环球科技运作有限责任公司 Dynamic obstacle avoidance in a robotic system
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
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
CN109960880A (en) * 2019-03-26 2019-07-02 上海交通大学 A kind of industrial robot obstacle-avoiding route planning method based on machine learning
CN110640736A (en) * 2019-10-23 2020-01-03 南京工业大学 Mechanical arm motion planning method for intelligent manufacturing

Patent Citations (12)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20140334714A1 (en) * 2011-04-21 2014-11-13 Seiko Epson Corporation Collision detection system, robotic system, collision detection method and program
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
CN105710888A (en) * 2014-10-17 2016-06-29 通用汽车环球科技运作有限责任公司 Dynamic obstacle avoidance in a robotic system
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
CN109960880A (en) * 2019-03-26 2019-07-02 上海交通大学 A kind of industrial robot obstacle-avoiding route planning method based on machine learning
CN110640736A (en) * 2019-10-23 2020-01-03 南京工业大学 Mechanical arm motion planning method for intelligent manufacturing

Cited By (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112828890A (en) * 2021-01-04 2021-05-25 武汉晴川学院 Mechanical arm track planning method and device, electronic equipment and storage medium
CN112828890B (en) * 2021-01-04 2022-08-02 武汉晴川学院 Mechanical arm track planning method and device, electronic equipment and storage medium
CN112975986A (en) * 2021-03-25 2021-06-18 珞石（北京）科技有限公司 Mechanical arm point-to-point trajectory planning method and device based on radial basis function
CN114489055A (en) * 2021-12-31 2022-05-13 中国科学技术大学 Robot multitask movement implementation method, medium and equipment based on sequential logic
CN114310918A (en) * 2022-03-14 2022-04-12 珞石（北京）科技有限公司 Mechanical arm track generation and correction method under man-machine cooperation
CN114721412A (en) * 2022-03-16 2022-07-08 北京理工大学 Unmanned aerial vehicle trajectory 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
CN115609595A (en) * 2022-12-16 2023-01-17 北京中海兴达建设有限公司 Trajectory planning method, device and equipment of mechanical arm and readable storage medium

Also Published As

Publication number Publication date
CN111633646B (en) 2021-08-27

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
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
Walker et al. Robot-human handovers based on trust
Colbaugh et al. Adaptive tracking control of manipulators: Theory and experiments
Wang et al. An adaptive imitation learning framework for robotic complex contact-rich insertion tasks
Yang et al. Variable impedance skill learning for contact-rich manipulation
Liu et al. Dynamic obstacle avoidance for cable-driven parallel robots with mobile bases via sim-to-real reinforcement learning
Zhou et al. Neural network-based variable stiffness impedance control for internal/external forces tracking of dual-arm manipulators under uncertainties
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
Dong et al. A dmp-based online adaptive stiffness adjustment method
Papageorgiou et al. Learning by demonstration for constrained tasks
Buss et al. Advanced telerobotics: Dual-handed and mobile remote manipulation
Jia et al. Research and implementation of complex task based on DMP
Fang et al. Learning from wearable-based teleoperation demonstration
Wang et al. A kinect-based trajectory capturing and tracking scheme for upper-limb robot-aided rehabilitation
Hassanein et al. Auto-generating fuzzy system modelling of physical systems
Mira et al. Investigating Various Force Control Techniques Effect on a Flexible Continuum Robot
Yang et al. Learning impedance actions for safe reinforcement learning in contact-rich tasks
Bodor et al. Predictive control of robot manipulators with flexible joints
Lukic et al. Online Cartesian Compliance Shaping of Redundant Robots in Assembly Tasks. Machines 2023, 11, 35
Wang et al. Position-posture Control Strategy for Planar Underactuated Manipulators with Second-order Nonholonomic Constraint
Liu et al. A Human-Like Learning Framework of Robot Interaction Skills Based on Environmental Dynamics

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