CN113843793B - Mobile redundant mechanical arm model predictive control method with obstacle avoidance function - Google Patents
Mobile redundant mechanical arm model predictive control method with obstacle avoidance function Download PDFInfo
- Publication number
- CN113843793B CN113843793B CN202111117973.XA CN202111117973A CN113843793B CN 113843793 B CN113843793 B CN 113843793B CN 202111117973 A CN202111117973 A CN 202111117973A CN 113843793 B CN113843793 B CN 113843793B
- Authority
- CN
- China
- Prior art keywords
- mechanical arm
- mobile
- redundant
- redundant mechanical
- obstacle
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 81
- 239000012636 effector Substances 0.000 claims description 22
- 239000011159 matrix material Substances 0.000 claims description 20
- 238000005457 optimization Methods 0.000 claims description 4
- 230000000295 complement effect Effects 0.000 claims description 3
- 238000013507 mapping Methods 0.000 claims description 3
- 238000005070 sampling Methods 0.000 claims description 3
- 238000004088 simulation Methods 0.000 description 35
- 230000007547 defect Effects 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 description 2
- 230000015556 catabolic process Effects 0.000 description 1
- 238000004321 preservation Methods 0.000 description 1
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
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Manipulator (AREA)
- Numerical Control (AREA)
Abstract
The invention provides a mobile redundant mechanical arm model predictive control method with an obstacle avoidance function, which belongs to the technical field of mechanical arm control and comprises the following steps: s1: aiming at the movable redundant mechanical arm, a kinematic model of the movable redundant mechanical arm is constructed; s2: according to the kinematic model of the mobile redundant mechanical arm, a state space model is constructed, and the future state and the future output of the system are predicted; s3: aiming at the track tracking problem of the mobile redundant mechanical arm, designing and optimizing performance indexes, and designing system state constraint and input constraint according to joint limits of the mechanical arm; s4: aiming at the problem of safe operation of the mobile redundant mechanical arm in an obstacle environment, a obstacle avoidance scheme is designed; s5: and (3) designing a prediction control method of the mobile redundant mechanical arm model, and completing track tracking control of the mobile redundant mechanical arm under the driving of the mobile redundant mechanical arm model. The invention can realize the track tracking control of the movable redundant mechanical arm in the obstacle environment and ensure the accuracy and the safety of the control of the movable redundant mechanical arm.
Description
Technical Field
The invention relates to the technical field of mechanical arm control, in particular to a mobile redundant mechanical arm model predictive control method with an obstacle avoidance function.
Background
Redundant robotic arms with more degrees of freedom than required are powerful tools to free up productivity and have been widely used in the industry. For mobile redundant robotic arms, inverse kinematics studies, i.e., acquisition of joint information based on the position of the end effector, are central to their motion control. Motion control based on quadratic programming is a mainstream method, but it requires the introduction of additional parameters and the preservation of certain margins when dealing with different levels of joint constraints, which reduces the feasible area of joint angles. From the point of view of handling multiple constraints, model predictive control is applicable to motion control of mobile redundant robotic arms. The optimization problem in model predictive control may contain a number of constraints, which may be expressed in an original form. That is, the joint constraint of the mobile redundant mechanical arm acts on the model predictive control scheme in the original form, so that the original feasible region of the joint angle is ensured.
In order to ensure safe operation of the mobile redundant manipulator, attention should be paid to avoiding obstacles in its working space in addition to ensuring joint constraint. At present, some obstacle avoidance methods suitable for redundant mechanical arms are already presented. Chinese patent CN108772835a published in 11.9.2018 discloses a method for avoiding obstacles and physical limits. Chinese patent CN112605996A published in 4/6 of 2021 discloses a model-free collision avoidance control method for a redundant mechanical arm. It should be noted that the existing obstacle avoidance method has the defects of smaller feasible range of joint angles, poorer track tracking performance and the like, and still has a larger improvement space.
Against such a background, it is needed to provide a predictive control method for a mobile redundant manipulator model with an obstacle avoidance function, so as to improve the accuracy and safety of the control of the mobile redundant manipulator.
Disclosure of Invention
Aiming at the defects of smaller joint angle feasible region, poorer track tracking performance and the like in the existing mechanical arm control and obstacle avoidance technology, the invention provides a mobile redundant mechanical arm model prediction control method with an obstacle avoidance function, so as to improve the accuracy and safety of mobile redundant mechanical arm control.
The technical scheme adopted by the invention is as follows:
a mobile redundant mechanical arm model prediction control method with obstacle avoidance function can realize track tracking control of a mobile redundant mechanical arm in an obstacle environment, and comprises the following steps:
s1: aiming at the movable redundant mechanical arm, a kinematic model of the movable redundant mechanical arm is constructed;
s2: according to the kinematic model of the mobile redundant mechanical arm, a state space model is constructed, and the future state and the future output of the system are predicted;
s3: aiming at the track tracking problem of the mobile redundant mechanical arm, designing and optimizing performance indexes, and designing system state constraint and input constraint according to joint limits of the mechanical arm;
s4: aiming at the problem of safe operation of the mobile redundant mechanical arm in an obstacle environment, a obstacle avoidance scheme is designed;
s5: and (3) designing a prediction control method of the mobile redundant mechanical arm model, and completing track tracking control of the mobile redundant mechanical arm under the driving of the mobile redundant mechanical arm model.
Further, the kinematic model of the mobile redundant manipulator described in step S1 is built as
Wherein,,for moving the position of the end effector of the redundant manipulator in the world coordinate system; w is the rotation angle of the mobile platform; />The position of the end effector of the redundant manipulator in its base coordinate system;the joint angle of the redundant mechanical arm is 6 degrees of freedom; />The position of the connection point of the redundant mechanical arm and the mobile platform in the world coordinate system is determined; />For the joint angle of the rotation angle of the drive wheel of the mobile platform and the joint angle of the redundant manipulator, wherein +.>Representing the rotation angle of the two driving wheels of the mobile platform;mapping functions for moving joint angles of redundant robotic arms to their end effector positions; />For moving the end effector of the redundant manipulator at a speed in world coordinate system of +.>Derivative with respect to time t;jacobian matrix for the mobile redundant manipulator; />For moving the angular velocity of the redundant manipulator, it is the derivative of q with respect to time t.
Further, the step S2 is specifically
S201: according to a kinematic model of the mobile redundant mechanical arm, a state space model of the mobile redundant mechanical arm at k time is built, wherein k represents an update index of t=kδ seconds at the current time, and δ is a sampling interval;
s202: predicting system states in N time domains in the future according to a state space model of the mobile redundant mechanical arm at the moment k, whereinRepresenting a prediction time domain;
s203: and predicting system output in N time domains in the future according to a state space model of the mobile redundant mechanical arm at the moment k.
Further, the state space model of the mobile redundant manipulator at the k time in step S201 is established as
Wherein,,for state variables of the system->And->The joint angle of the movable redundant mechanical arm at the moment k is shown; x is x j And x j+1 System state variables respectively representing front and rear moments;is an input variable of the system; />For the output variables of the system, +.>And->The position of the end effector of the movable redundant mechanical arm at the moment k is shown;the jacobian matrix at the time k is used for moving the redundant mechanical arm.
Further, the system states in the future N time domains described in step S202 are predicted asWherein,,
i=1, 2, …, N for the system state at time k+i in the future; />For system input at future time k+i, i=0, 1, …, N u -1, and->Representing a control time domain; i is an identity matrix.
Further go forwardStep S203, predicting the system output in the future N time domains asWherein,,
for the system output at the future time k+i, i=1, 2, …, N.
Further, the optimization performance index for tracking control of the mobile redundant manipulator track in step S3 is designed as followsWherein (1)>And->A symmetric weight matrix is positively determined;and->Representing the euclidean norm,i=1, 2, …, N for the expected trajectory of the moving redundant robot end effector at the future time k+i.
Further, the system state constraint and the input constraint described in step S3 are respectively designed as followsAndwherein,,
and q + And q - The upper and lower limits of the joint angle of the movable redundant mechanical arm;and->For moving the upper and lower limits of the joint angular velocity of the redundant manipulator.
Further, the step S4 is specifically
S401: determining a safety distance between the obstacle and the mobile redundant manipulator, wherein the internal safety threshold is d 1 The distance between the movable redundant mechanical arm and the obstacle is the collision distance; an external safety threshold value of d 2 Is the distance for moving redundant mechanical arm to avoid obstacle, and in general, d is taken 2 ≥2d 1 ；
S402: determining a critical point on the mobile redundant manipulator, the distance between the critical point and the obstacle being smaller than the outer safety threshold d 2 ；
S403: and designing an obstacle avoidance method, and endowing the critical point with a proper escape speed so as to keep the critical point away from the obstacle.
Further, the obstacle avoidance method in step S403 is designed as follows
Wherein,,s＝[x C -x O ，y C -y O ，z C -z O ] T ；(·) T representing a transpose operation on the matrix or vector; the point C is a critical point on the mechanical arm, and the coordinates of the critical point are (x C ，y C ，z C ) The O-point is an obstacle, and its coordinates are (x O ，y O ，z O )；G C Jacobian matrix, which is the critical point C;
d represents the distance between the critical point and the obstacle.
In particular, the obstacle avoidance method described in step S403 is superior to the conventional obstacle avoidance method (CN 112605996A, CN108772835 a), and is mainly embodied in the following two aspects: 1) The obstacle avoidance method described in step S403 adopts a vector dot product to design the escape speed of the critical point. Compared with the traditional obstacle avoidance method, the method greatly widens the pointing range of escape speed, namely increases the feasible area of the mechanical arm, and ensures that the mechanical arm can avoid the obstacle as much as possible without collapsing. 2) The obstacle avoidance method in step S403 gives up the use of the symbol function in the conventional obstacle avoidance method, and further improves the tracking accuracy of the end effector of the mechanical arm in the obstacle avoidance process.
Further, the step S5 is specifically
S501: the method constructs a model predictive control scheme with obstacle avoidance function for tracking and controlling the track of the mobile redundant mechanical arm as follows
Minimization:
model constraint:
wherein,,
s502: the model predictive control scheme of the mobile redundant manipulator is converted into a standard quadratic programming form:
minimization:
model constraint: sz is less than or equal to f,
wherein,, m＝16N+16N u +1，n＝8N u ；
s503: and converting the model predictive control scheme of the mobile redundant mechanical arm into a nonlinear equation by using a Lagrange multiplier method and a nonlinear complementary problem function, and further designing a solver of the model predictive control scheme of the mobile redundant mechanical arm based on the equation.
Further, the nonlinear equation in step S503 is K χ + ψ=0, wherein,
ρ∈(0，1)，/>is Lagrangian multiplier +.>a + ＝max{0，a}，/> Representing Hadamard product, ->The solver of the mobile redundant mechanical arm model predictive control scheme is +.>Wherein γ=ατ, ++>For the design parameters, τ is the step size, representing Hadamard division, ">
Still further, for the input variables calculated by the solverThe first input, namely the input u (k) at the moment k, is taken and acts on the mobile redundant mechanical arm system to complete the track tracking control of the mobile redundant mechanical arm.
Drawings
For further description of the objects and technical solutions of the present invention, the present invention is illustrated by the following drawings:
FIG. 1 is a flow chart of a predictive control method for a mobile redundant manipulator model with obstacle avoidance function;
FIG. 2 (a) is a front view of a mobile redundant manipulator;
FIG. 2 (b) is a side view of a mobile redundant manipulator;
FIG. 3 (a) is a graph of three-dimensional obstacle avoidance results of simulation experiment one without using an obstacle avoidance method;
FIG. 3 (b) is a graph of the distance between the critical point of simulation experiment one and an obstacle without using the obstacle avoidance method;
FIG. 4 (a) is a two-dimensional plan view of the actual and desired trajectories of a mobile redundant robotic arm end effector of simulation experiment one using the obstacle avoidance method of the present invention;
FIG. 4 (b) is a three-dimensional obstacle avoidance result graph of simulation experiment one in the case of using the obstacle avoidance method of the present invention;
FIG. 4 (c) is a graph of the distance between the critical point of simulation experiment one and the obstacle using the obstacle avoidance method of the present invention;
FIG. 4 (d) is a graph of tracking errors in three directions X, Y, Z of simulation experiment one using the obstacle avoidance method of the present invention;
FIG. 5 (a) is a three-dimensional obstacle avoidance result diagram of simulation experiment two in the case of using the obstacle avoidance method of the present invention;
FIG. 5 (b) is the tracking error in three directions X, Y, Z of simulation experiment two in the case of using the obstacle avoidance method of the present invention;
FIG. 5 (c) is a graph of the distance between the critical point of simulation experiment two and an obstacle using the obstacle avoidance method of the present invention;
FIG. 5 (d) is a graph of the distance between the critical point of simulation experiment two and an obstacle without using the obstacle avoidance method;
FIG. 6 (a) is a graph of the distance between the critical point of simulation experiment one and an obstacle without using the obstacle avoidance method;
FIG. 6 (b) is a graph of tracking errors in three directions X, Y, Z of simulation experiment one without using the obstacle avoidance method;
FIG. 6 (c) is a graph of the distance between the critical point of the simulation experiment one and the obstacle in the case of the obstacle avoidance method of the national patent CN 112605996A;
fig. 6 (d) is a graph of tracking errors in X, Y, Z three directions of simulation experiment one in the case of the obstacle avoidance method of the national patent CN112605996 a;
FIG. 6 (e) is a graph of the distance between the critical point of the simulation experiment one and the obstacle in the case of the obstacle avoidance method of the national patent CN 108772835A;
FIG. 6 (f) is a graph of tracking errors in X, Y, Z three directions of simulation experiment one in the case of the obstacle avoidance method of the national patent CN 108772835A;
FIG. 6 (g) is a graph of the distance between the critical point of simulation experiment one and the obstacle using the obstacle avoidance method of the present invention;
fig. 6 (h) is a graph of tracking errors in three directions X, Y, Z of simulation experiment one in the case of using the obstacle avoidance method of the present invention.
Detailed Description
The present invention will be described in detail below with reference to the accompanying drawings and examples in order to make the objects and technical solutions of the present invention more clear.
The embodiment is based on a mobile platform carrying a 6-degree-of-freedom redundant manipulator. Wherein the initial joint angles of the redundant mechanical arms are set to [ pi/12, pi/2, pi/12, pi/3, -pi/12, pi/6] T The initial angle of the left driving wheel and the right driving wheel of the moving platform is set to be 0,0] T Radians, i.e. q (0) = [0, pi/12, pi/2, pi/12, pi/3, -pi/12, pi/6] T Radian, the upper and lower limits of the joint angle are set to be-q - ＝q + ＝[+∞，+∞，π，π，π，π，π，π] T Radian, the upper and lower limits of the combined angular velocity are set asRadians/second; the sampling interval δ is set to 0.1 seconds; prediction time domain N and control time domain N u Are all set to 3; the weight matrix Q is set to 10 5 I, a step of I; the weight matrix V is set to 10 -4 I, a step of I; inner safety threshold d 1 Setting the water content to be 0.1 meter; external safety threshold d 2 Setting the water content to be 0.2 meter; the remaining relevant parameters were set as follows: ρ=0.95, τ=0.001, α=300.
Considering the problems of smaller feasible area of joint angle, poor track tracking performance and the like in the existing mechanical arm control and obstacle avoidance technology, the embodiment provides a mobile redundant mechanical arm model prediction control method with an obstacle avoidance function, and the method comprises the following steps in combination with fig. 1:
step one: according to the structure of the mobile redundant manipulator, as shown in FIG. 2, a kinematic model is built as
Wherein,,for moving the position of the end effector of the redundant manipulator in the world coordinate system; omega is the rotation angle of the mobile platform; />The position of the end effector of the redundant manipulator in its base coordinate system;the joint angle of the redundant mechanical arm is 6 degrees of freedom; />The position of the connection point of the redundant mechanical arm and the mobile platform in the world coordinate system is determined; />For rotating the driving wheel of the mobile platformJoint angle of corner and redundant arm joint angle, wherein +.>Representing the rotation angle of the two driving wheels of the mobile platform;mapping functions for moving joint angles of redundant robotic arms to their end effector positions; />For moving the end effector of the redundant manipulator at a speed in world coordinate system of +.>Derivative with respect to time t;jacobian matrix for the mobile redundant manipulator; />For moving the angular velocity of the redundant manipulator, it is the derivative of q with respect to time t.
Step two: and constructing a state space model of the mobile redundant mechanical arm according to a kinematic model of the mobile redundant mechanical arm, and predicting the future state and the future output of the system.
In particular to
(201) According to the kinematic model of the mobile redundant mechanical arm, constructing a state space model of the mobile redundant mechanical arm at the moment k as
Wherein,,for state variables of the system->And->The joint angle of the movable redundant mechanical arm at the moment k is shown; x is x j And x j+1 System state variables respectively representing front and rear moments; />Is an input variable of the system; />As an output variable of the system,and->The position of the end effector of the movable redundant mechanical arm at the moment k is shown;the Jacobian matrix at the moment k is used for moving the redundant mechanical arm;
(202) Predicting the system state in the future 3 time domains asWherein,,
for future k+iSystem state at time, i=1, 2,3; />For system inputs at future k+i times, i=0, 1,2; i is an identity matrix;
(203) Predicting system output in 3 future time domains asWherein (1)>
I=1, 2,3 for the system output at the future time k+i.
Step three: aiming at the track tracking problem of the mobile redundant mechanical arm, the performance index is designed and optimized, and the system state constraint and the input constraint are designed according to the joint limit of the mechanical arm.
In particular to
(301) Aiming at the track tracking problem of the mobile redundant mechanical arm, the design optimization performance index is as followsWherein (1)>And->A symmetric weight matrix is positively determined; />And->Representing the euclidean norm,i=1, 2,3 for the expected trajectory of the mobile redundant robot arm end effector at time k+i in the future;
(302) According to the joint limit of the mechanical arm, the system state constraint and the input constraint are designed as followsAnd->Wherein,,
and q + And q - To move the joint angular upper and lower limits of the redundant robot,and->For moving the upper and lower limits of the joint angular velocity of the redundant manipulator.
Step four: aiming at the problem of safe operation of the mobile redundant mechanical arm in an obstacle environment, an obstacle avoidance scheme is designed.
In particular to
(401) Determining a safety distance between the obstacle and the mobile redundant robot arm, wherein an inner safety threshold d 1 External safety threshold d=0.1 meters 2 =0.2 meters;
(402) Determining a critical point on the mobile redundant manipulator, the distance between the critical point and the obstacle being smaller than the outer safety threshold d 2 ；
(403) And designing an obstacle avoidance method, and endowing the critical point with a proper escape speed so as to keep the critical point away from the obstacle.
Further, the obstacle avoidance method of step (403) is designed to
Wherein,,s＝[x C -x O ，y C -y O ，z C -z O ] T ；(·) T representing a transpose operation on the matrix or vector; the point C is a critical point on the mechanical arm, and the coordinates of the critical point are (x C ，y C ，z C ) The method comprises the steps of carrying out a first treatment on the surface of the The O-point is an obstacle, and its coordinates are (x O ，y O ，z O )；G C Jacobian matrix, which is the critical point C; />
d represents the distance between the critical point and the obstacle.
Step five: and (3) designing a prediction control method of the mobile redundant mechanical arm model, and completing track tracking control of the mobile redundant mechanical arm under the driving of the mobile redundant mechanical arm model.
In particular to
(501) The method constructs a model predictive control scheme with obstacle avoidance function for tracking and controlling the track of the mobile redundant mechanical arm as follows
Minimization:
model constraint:
wherein,,
(502) The model predictive control scheme of the mobile redundant manipulator is converted into a standard quadratic programming form:
minimization:
model constraint: sz is less than or equal to f,
wherein,,
(503) And converting the model predictive control scheme of the mobile redundant mechanical arm into a nonlinear equation by using a Lagrange multiplier method and a nonlinear complementary problem function, and further designing a solver of the model predictive control scheme of the mobile redundant mechanical arm based on the equation.
Further, the nonlinear equation in the step (503) is K χ + ψ=0, wherein,
in order to be a lagrange multiplier,a + ＝max{0，a}，/> representing Hadamard product, ->The solver of the mobile redundant mechanical arm model predictive control scheme is +.>Wherein,, representing Hadamard division
Further, for the input variables calculated by the solverThe first input, namely the input u (k) at the moment k, is taken and acts on the mobile redundant mechanical arm system to complete the track tracking control of the mobile redundant mechanical arm.
In this embodiment, 120 seconds of simulation is performed on MATLAB software, and simulation results are shown in fig. 3, fig. 4, fig. 5, and fig. 6. The simulation example is divided into a simulation experiment I and a simulation experiment II, wherein the simulation experiment I tracks a kidney-shaped line for the mobile redundant mechanical arm, and only one obstacle exists in the environment, and the coordinates of the obstacle are (3.23,0.03,0.78) meters; simulation experiment IIA nice Mei Desi spiral is tracked for moving redundant robotic arms, and there are two obstacles in the environment whose coordinates are (2.73, -0.01,0.78) meters, (3.92, -1.93,0.78) meters, respectively. Fig. 3 is a simulation result of simulation experiment one without using the obstacle avoidance method, in which fig. 3 (a) is a three-dimensional obstacle avoidance result diagram and fig. 3 (b) is a distance between a critical point and an obstacle. As can be seen from fig. 3, the redundant robot arm collides with the obstacle without using the obstacle avoidance method. FIG. 4 is a simulation result of simulation experiment one in the case of using the obstacle avoidance method of the present invention, in which FIG. 4 (a) is a two-dimensional plan view of the actual and desired trajectories of the mobile redundant robot end effector, FIG. 4 (b) is a three-dimensional obstacle avoidance result view, FIG. 4 (c) is the distance between the critical point and the obstacle, FIG. 4 (d) is the tracking error in three directions of X, Y, Z, respectively defined by e X 、e Y 、e Z And (3) representing. As can be seen from fig. 4, in the case of using the obstacle avoidance method of the present invention, the distance between the critical point and the obstacle is greater than the inner safety threshold d 1 I.e. the redundant robot arm avoids collisions with obstacles. Fig. 5 (a) - (c) are simulation results of simulation experiment two in the case of using the obstacle avoidance method of the present invention, wherein fig. 5 (a) is a three-dimensional obstacle avoidance result graph, fig. 5 (b) is a tracking error in three directions of X, Y, Z, and fig. 5 (c) is a distance between a critical point and an obstacle; fig. 5 (d) is a simulation result of a simulation experiment two, i.e., a distance between a critical point and an obstacle, without using the obstacle avoidance method. As can be seen from fig. 5, in the case that the obstacle avoidance method is not used, the redundant mechanical arm collides with the obstacle for a plurality of times; in the case of using the obstacle avoidance method of the present invention, the distance between the critical point and the obstacle is greater than the inner safety threshold d 1 I.e. the redundant robot arm avoids collisions with all obstacles.
Fig. 6 is a simulation result of simulation experiment one in the case of not using the obstacle avoidance method, using the obstacle avoidance method of chinese patent CN112605996a, using the obstacle avoidance method of chinese patent CN108772835A, using the obstacle avoidance method of the present invention, wherein fig. 6 (a) - (b) are the distance between the critical point and the obstacle and the tracking error in X, Y, Z three directions, respectively, in the case of not using the obstacle avoidance method, fig. 6 (c) - (d) are the distance between the critical point and the obstacle and the tracking error in X, Y, Z three directions, respectively, in the case of using the obstacle avoidance method of chinese patent CN112605996a, fig. 6 (e) - (f) are the distance between the critical point and the obstacle and the tracking error in X, Y, Z three directions, respectively, in the case of using the obstacle avoidance method of the present invention, and fig. 6 (g) - (h) are the distance between the critical point and the obstacle and the tracking error in X, Y, Z three directions, respectively. As can be seen from fig. 6, the obstacle avoidance method of chinese patent CN112605996a has a smaller feasible range of joint angles, resulting in a breakdown of the obstacle avoidance process; the obstacle avoidance method of the Chinese patent CN108772835A generates larger tracking error; the obstacle avoidance method has small tracking error under the condition of realizing obstacle avoidance.
In summary, the method of the invention effectively realizes the track tracking control of the mobile redundant mechanical arm in the obstacle environment, and improves the accuracy and safety of the control of the mobile redundant mechanical arm.
Finally, it should be noted that the above-mentioned preferred embodiments are merely for illustrating the technical solution of the present invention and not for limiting the same, and that although the present invention has been described in detail by means of the above-mentioned preferred embodiments, it should be understood by those skilled in the art that various changes in form and details may be made therein without departing from the scope of the present invention as defined by the appended claims.
Claims (1)
1. A mobile redundant mechanical arm model prediction control method with obstacle avoidance function is characterized by comprising the following steps:
s1: aiming at the movable redundant mechanical arm, a kinematic model of the movable redundant mechanical arm is constructed;
the kinematic model of the mobile redundant mechanical arm is established as
Wherein,,for moving the position of the end effector of the redundant manipulator in the world coordinate system; omega is the rotation angle of the mobile platform; />The position of the end effector of the redundant manipulator in its base coordinate system; />The joint angle of the redundant mechanical arm is 6 degrees of freedom; />The position of the connection point of the redundant mechanical arm and the mobile platform in the world coordinate system is determined; />For the joint angle of the rotation angle of the drive wheel of the mobile platform and the joint angle of the redundant manipulator, wherein +.>Representing the rotation angle of the two driving wheels of the mobile platform; />Mapping functions for moving joint angles of redundant robotic arms to their end effector positions; />For moving the end effector of the redundant manipulator at a speed in world coordinate system of +.>Derivative with respect to time t; />For movingJacobian matrix of the dynamic redundant manipulator; />For the angular velocity of the mobile redundant manipulator, q is the derivative of q with respect to time t;
s2: according to the kinematic model of the mobile redundant mechanical arm, a state space model is constructed, and the future state and the future output of the system are predicted, in particular
S201: according to the kinematic model of the mobile redundant mechanical arm, constructing a state space model of the mobile redundant mechanical arm at the moment k as
Where k represents an update index of the current time t=kδ seconds, δ being a sampling interval;for state variables of the system->And->The joint angle of the movable redundant mechanical arm at the moment k is shown; x is x j And x j+1 System state variables respectively representing front and rear moments; />Is an input variable of the system;for the output variables of the system, +.>And->The position of the end effector of the movable redundant mechanical arm at the moment k is shown; />The Jacobian matrix at the moment k is used for moving the redundant mechanical arm;
s202: predicting the system state in N time domains in the future asWherein,,
i=1, 2, …, N, and +.>Representing a prediction time domain; />For system input at future time k+i, i=0, 1, …, N u -1, and->Representing a control time domain; i is an identity matrix;
s203: predicting in the future according to a state space model of the mobile redundant mechanical arm at the moment kThe system output in N time domains isWherein (1)>
For the system output at future time k+i, i=1, 2, …, N;
s3: aiming at the track tracking problem of a mobile redundant mechanical arm, the performance index is designed and optimized, and system state constraint and input constraint are designed according to the joint limit of the mechanical arm, specifically
S301: aiming at the track tracking problem of the mobile redundant mechanical arm, the design optimization performance index is as followsWherein (1)>And->A symmetric weight matrix is positively determined; />Andrepresenting euclidean norms, +.> I=1, 2, …, N for the expected trajectory of the mobile redundant robot end effector at time k+i in the future;
s302: according to the joint limit of the mechanical arm, the system state constraint and the input constraint are designed as followsAndwherein,,
and q + And q - To move the joint angular upper and lower limits of the redundant robot,and->Upper and lower limits for joint angular velocity of the mobile redundant manipulator;
s4: aiming at the safety operation problem of the mobile redundant mechanical arm in the obstacle environment, an obstacle avoidance scheme of the mobile redundant mechanical arm is designed, in particular
S401: determining a safety distance between the obstacle and the mobile redundant manipulator, wherein the internal safety threshold is d 1 The distance between the movable redundant mechanical arm and the obstacle is the collision distance; an external safety threshold value of d 2 Is the distance for moving redundant mechanical arm to avoid obstacle, and in general, d is taken 2 ≥2d 1 ；
S402: determining a critical point on the mobile redundant manipulator, the distance between the critical point and the obstacle being smaller than the outer safety threshold d 2 ；
S403: designing an obstacle avoidance method, and endowing a critical point with a proper escape speed to keep the critical point away from an obstacle;
the obstacle avoidance method is designed as
Wherein,,s＝[x C -x O ，y C -y O ，z C -z O ] T ；(·) T representing a transpose operation on the matrix or vector; the point C is a critical point on the mechanical arm, and the coordinates of the critical point are (x C ，y C ，z C ) The method comprises the steps of carrying out a first treatment on the surface of the The O-point is an obstacle, and its coordinates are (x O ，y O ，z O ），G C Jacobian matrix, which is the critical point C;
d represents the distance between the critical point and the obstacle;
s5: a predictive control method for a mobile redundant mechanical arm model is designed, and track tracking control of the mobile redundant mechanical arm is completed under the drive of the predictive control method, specifically
S501: the method constructs a model predictive control scheme with obstacle avoidance function for tracking and controlling the track of the mobile redundant mechanical arm as follows
Minimization of：
Model constraint:
wherein,,
s502: the model predictive control scheme of the mobile redundant manipulator is converted into a standard quadratic programming form:
minimization:
model constraint: sz is less than or equal to f,
wherein,, m＝16N+16N u +1，n＝8N u ；
s503: converting a model predictive control scheme of the mobile redundant mechanical arm into a nonlinear equation by utilizing a Lagrangian multiplier method and a nonlinear complementary problem function, and further designing a solver of the model predictive control scheme of the mobile redundant mechanical arm based on the equation;
the nonlinear equation is kχ+ψ=0, wherein,
ρ∈(0，1)，/>is Lagrangian multiplier +.>a + ＝max{0，a}，/> Representing Hadamard product, ->The solver of the mobile redundant mechanical arm model prediction control scheme is χ j+1 ＝χ j -γ(K+M j ) -1 (Kχ j +ψ j ) Wherein γ=ατ, +.>For the design parameters, τ is the step size, representing Hadamard division, "> Further, for the input variable calculated by the solver +.>The first input, namely the input u (k) at the moment k, is taken and acts on the mobile redundant mechanical arm system to complete the track tracking control of the mobile redundant mechanical arm.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111117973.XA CN113843793B (en) | 2021-09-21 | 2021-09-21 | Mobile redundant mechanical arm model predictive control method with obstacle avoidance function |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111117973.XA CN113843793B (en) | 2021-09-21 | 2021-09-21 | Mobile redundant mechanical arm model predictive control method with obstacle avoidance function |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN113843793A CN113843793A (en) | 2021-12-28 |
| CN113843793B true CN113843793B (en) | 2023-07-21 |
Family
ID=78979461
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202111117973.XA Active CN113843793B (en) | 2021-09-21 | 2021-09-21 | Mobile redundant mechanical arm model predictive control method with obstacle avoidance function |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113843793B (en) |
Families Citing this family (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114571461B (en) * | 2022-03-24 | 2023-08-11 | 合肥工业大学 | Three-degree-of-freedom three-dimensional parallel robot track tracking control algorithm based on Udwadia-Kalaba method |
| CN115256395B (en) * | 2022-08-17 | 2024-06-18 | 北京理工大学 | Model uncertain robot safety control method based on control obstacle function |
Family Cites Families (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN104842355B (en) * | 2015-01-20 | 2016-08-17 | 西北工业大学 | The MIXED INTEGER forecast Control Algorithm of the lower redundant space robot of avoidance constraint |
| CN106625666B (en) * | 2016-12-16 | 2019-03-01 | 广州视源电子科技股份有限公司 | Control method and device of redundant mechanical arm |
| CN107662211B (en) * | 2017-10-16 | 2020-09-08 | 西北工业大学 | Space robot prediction control method based on quantum particle swarm algorithm |
| CN111975771A (en) * | 2020-07-30 | 2020-11-24 | 华南理工大学 | Mechanical arm motion planning method based on deviation redefinition neural network |
| CN113276121B (en) * | 2021-05-31 | 2022-08-09 | 华南理工大学 | Redundant manipulator moving obstacle avoiding method based on quadratic programming |
-
2021
- 2021-09-21 CN CN202111117973.XA patent/CN113843793B/en active Active
Also Published As
| Publication number | Publication date |
|---|---|
| CN113843793A (en) | 2021-12-28 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN113843793B (en) | Mobile redundant mechanical arm model predictive control method with obstacle avoidance function | |
| US20220009096A1 (en) | Inverse kinematics solving method for redundant robot and redundant robot and computer readable storage medium using the same | |
| CN107490965B (en) | Multi-constraint trajectory planning method for space free floating mechanical arm | |
| CN109108942B (en) | Mechanical arm motion control method and system based on visual real-time teaching and adaptive DMPS | |
| US20210325894A1 (en) | Deep reinforcement learning-based techniques for end to end robot navigation | |
| US9592606B2 (en) | Method and control means for controlling a robot | |
| CN104760041B (en) | A kind of Obstacle avoidance motion planning method based on impact degree | |
| WO2018176593A1 (en) | Local obstacle avoidance path planning method for unmanned bicycle | |
| CN109807886A (en) | A kind of space non-cooperative target based on prediction arrests strategy | |
| CN107966907B (en) | Obstacle avoidance solution applied to redundant manipulator | |
| CN111522351B (en) | Three-dimensional formation and obstacle avoidance method for underwater robot | |
| CN108897321A (en) | Based on navigating, the robot formation for following method can be changed formation control method and controller | |
| CN108762256A (en) | The method of relatively high speed barrier is evaded by a kind of robot | |
| CN111309002A (en) | Wheel type mobile robot obstacle avoidance method and system based on vector | |
| CN109739094A (en) | A kind of mobile robot trace tracking new method based on adaptive sliding-mode observer | |
| CN107272741A (en) | A kind of unmanned plane automatic obstacle-avoiding method based on maximum inscribed circle center of circle mobile vector | |
| CN110561419A (en) | arm-shaped line constraint flexible robot track planning method and device | |
| Jin et al. | Dynamic collision avoidance scheme for unmanned surface vehicles under complex shallow sea Environments | |
| CN113608445A (en) | Multi-agent obstacle avoidance and collision avoidance method | |
| Sun et al. | A GNN for repetitive motion generation of four-wheel omnidirectional mobile manipulator with nonconvex bound constraints | |
| Li et al. | A model predictive obstacle avoidance method based on dynamic motion primitives and a Kalman filter | |
| CN107717991A (en) | A kind of mechanical arm control accuracy cooperative optimization method | |
| CN113467461B (en) | Man-machine cooperation type path planning method under mobile robot unstructured environment | |
| CN111829528A (en) | Real-time path planning method and system for bionic gliding machine dolphin | |
| Sun et al. | Hybrid task constrained planner for robot manipulator in confined environment |
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 |