WO2017132905A1 - Method and apparatus for controlling motion system - Google Patents

Abstract

A method and apparatus (700) for controlling a motion system. The method comprises: determining a singularity prevention constraint condition of the motion system according to an operability index of the motion system, wherein the operability of the singularity prevention constraint condition constraining the motion system is greater than 0 (S210); determining a dynamic optimization model of the motion system according to the singularity prevention constraint condition (S220); determining a motion parameter of the motion system according to the dynamic optimization model (S230); and controlling the motion system according to the motion parameter (S240). The method and apparatus (700) for controlling a motion system can prevent a singular state from occurring in the motion system.

Description

Method and apparatus for controlling a motion system Technical field

The present invention relates to the field of automation technology and, more particularly, to a method and apparatus for controlling a motion system.

Background technique

The motion system consists of a set of rigid segments joined by joints that transform the relative angles and displacements between the joints to produce a specific shape or pose (position and attitude). In the motion system, the inverse kinematic programming problem is the process of determining the motion parameters of the active joint required to achieve a given position and attitude. For example, for a three-dimensional model of a human body, the inverse kinematics plan is how to set the angle of the wrist and elbow so that the hand reaches a waved posture from the relaxed position. A redundant motion system means that the motion system has a greater degree of freedom than the operation (motion) when performing a given task, and has great advantages in complex workspace operations, avoiding obstacles, and optimizing load distribution. Sex. In practical applications, redundant motion systems are highly autonomous, flexible and fault tolerant. However, such a system loses the joint freedom of the system at a certain attitude, so that the joint degree of freedom is equal to or even less than the degree of freedom of motion, resulting in a singular state, which can make many advantages of the redundant motion system. Lost.

The prior art cannot avoid the singular state of the motion system. If the singular state occurs, it can only be solved offline, and the fault tolerance is poor. Moreover, the closed form solution of the inverse kinematics plan can only be obtained by offline operation, and it is difficult to capture the environmental factors of real-time change, and the robustness is poor. In addition, the joint freedom of the joint has a joint limit, and the prior art cannot handle the planning problem in which the joint degree of freedom is constrained.

Summary of the invention

Embodiments of the present invention provide a method and apparatus for controlling a motion system that can prevent a singular state from occurring in a motion system.

In a first aspect, a method of controlling a motion system is provided, comprising:

Determining a singular constraint condition of the motion system according to an operability index of the motion system, the value of the operability preventing the singular constraint condition binding the motion system being greater than 0;

Determining a dynamic optimization model of the motion system according to the singular constraint prevention condition;

Determining a motion parameter of the motion system according to the dynamic optimization model;

The motion system is controlled based on the motion parameters.

The method for controlling a motion system according to an embodiment of the present invention determines a singular constraint condition of the motion system according to an operability index of the motion system, and then dynamically calculates the motion of the motion system according to the dynamic optimization model determined by the singular constraint prevention condition. The parameters can avoid the singular state of the motion system, thus enhancing the fault tolerance of the motion system.

In some possible implementations, the method further includes:

Determining the objective function according to the energy index and the speed index of the motion system;

Wherein, the dynamic optimization model of the motion system is determined according to the singular constraint prevention condition, including:

The dynamic optimization model of the motion system is determined according to the objective function and the singularity prevention condition.

In some possible implementations, the method further includes:

Determining a trajectory tracking constraint according to a motion reference trajectory of the motion system;

Determining an obstacle avoidance constraint according to the obstacle avoidance safety distance of the motion system, the obstacle avoidance constraint restricting the motion direction of the motion system away from the obstacle when the distance between the motion system and the obstacle tends to the obstacle avoidance distance The direction of the object;

Wherein, the dynamic optimization model of the motion system is determined according to the singular constraint prevention condition, including:

The dynamic optimization model of the motion system is determined according to the trajectory tracking constraint condition, the obstacle avoidance constraint condition, the objective function, and the singularity prevention condition.

Here, the dynamic optimization model can be determined based on one or more of the above indicators. Of course, it can also be other possible expectations.

In some possible implementations, the dynamic optimization model of the motion system is determined according to the trajectory tracking constraint, the obstacle avoidance constraint, the objective function, and the singularity prevention condition, including:

Determine the dynamic optimization model according to the following formula,

为该θ对时间t求导的导数的二范数的平方，该||·||₂表示二范数，该

为该θ对该t求导的导数的无穷范数的平方，该||·||_∞表示无穷范数，该

为该目标函数，该

为该轨迹追踪约束条件，该

为该运动系统的正运动学表达式r＝f(θ)对该t求导的表达式，该J(θ)为该f(θ)的雅可比矩阵，该

为该避障约束条件，该

是由障碍物O指向运动关节C的向量，该J_c(θ)为所述运动关节C对应的雅可比矩阵，该

为该防止奇异约束条件，该

该δ为大于0的预定阈值，该

该det表示矩阵行列式，该

为该θ的上限θ_max对该t求导的导数，该

为该θ的下限θ_min对该t求导的导数。Wherein, the motion system is a motion system with n joint degrees of freedom θ=(θ ₁ , θ ₂ , . . . , θ _n ),

The square of the norm of the derivative of which θ is derived from time t, the ||·|| ₂ represents a two norm,

For the square of the infinite norm of the derivative of the θ derived from t, the ||·|| _∞ represents an infinite norm,

For the objective function, the

Tracking constraints for the trajectory,

An expression for which t is derived for the positive kinematic expression r=f(θ) of the motion system, the J(θ) being the Jacobian matrix of the f(θ),

For the obstacle avoidance constraint,

Is a vector pointing from the obstacle O to the moving joint C, and the J _c (θ) is a Jacobian matrix corresponding to the moving joint C,

For this to prevent singular constraints,

The δ is a predetermined threshold greater than 0, the

The det represents a matrix determinant,

Deriving the derivative of t for the upper limit θ _{max of} θ,

The derivative of the derivative of t is the lower limit θ _{min of} θ.

In some possible implementations, determining the motion parameter of the motion system according to the dynamic optimization model includes:

The motion parameter of the motion system is determined according to the neural network model corresponding to the dynamic optimization model.

Here, after obtaining the dynamic optimization model, the motion system uses the neural network to solve the dynamic optimization model online, so as to obtain the optimal solution of the shutdown degree of freedom, which can avoid offline training without artificial adjustment of internal parameters.

In some possible implementations, determining the motion parameter of the motion system according to the neural network model corresponding to the dynamic optimization model, including:

The motion parameter is determined according to the following state equation of the neural network model,

该J(θ)^T为该J(θ)的转置矩阵，该A为

该A^T为该A的转置矩阵，该K为n×n矩阵，且该K矩阵的第k行第k列元素为1其余元素均为0，该b为

Wherein the μ and the ρ are state parameters of the state equation, the ε is a gain, and the g is

The J(θ) ^{T is} a transposed matrix of the J(θ), and the A is

The A ^{T is} a transposed matrix of the A, the K is an n×n matrix, and the kth row and the kth column element of the K matrix are 1 and the remaining elements are all 0, and the b is

In a second aspect, there is provided an apparatus for controlling a motion system for performing the method of any of the above first aspect or any of the possible implementations of the first aspect. In particular, the apparatus comprises means for performing the method of any of the above-described first aspect or any of the possible implementations of the first aspect.

In a third aspect, an apparatus for controlling a motion system is provided. The device includes a processor, a memory, and a communication interface. The processor is coupled to the memory and communication interface. The memory is for storing instructions for the processor to execute, and the communication interface is for communicating with other network elements under the control of the processor. When the processor executes the instructions stored by the memory, the execution causes the processor to perform the method of the first aspect or any of the possible implementations of the first aspect.

In a fourth aspect, a computer readable medium is provided for storing a computer program comprising instructions for performing the method of the first aspect or any of the possible implementations of the first aspect.

DRAWINGS

In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the drawings to be used in the embodiments of the present invention will be briefly described below. It is obvious that the drawings described below are only some embodiments of the present invention, Those skilled in the art can also obtain other drawings based on these drawings without paying any creative work.

Fig. 1 is a structural view showing a mechanical arm to which the technical solution of the embodiment of the present invention can be applied.

2 shows a schematic flow chart of a method of controlling a motion system in accordance with an embodiment of the present invention.

Figure 3 shows a schematic flow chart of a neural network based inverse kinematics plan.

Figure 4 shows a simulation result of the motion trajectory of the robot arm.

Fig. 5 is a graph showing the simulation results of the velocity information of the joint degrees of freedom of the robot arm.

Fig. 6 is a graph showing the simulation results of the manipulator's operability index.

Figure 7 shows a schematic block diagram of an apparatus for controlling a motion system in accordance with an embodiment of the present invention.

FIG. 8 is a block diagram showing an apparatus for controlling a motion system according to still another embodiment of the present invention.

detailed description

The technical solutions in the embodiments of the present invention are clearly and completely described in the following with reference to the accompanying drawings in the embodiments of the present invention. It is obvious that the described embodiments are a part of the embodiments of the present invention, but not all embodiments. All other embodiments obtained by those skilled in the art based on the embodiments of the present invention without creative efforts are within the scope of the present invention.

The technical solution of the present invention can be applied to various motion systems, for example, a robot (for example, PUMA560, etc.), an industrial robot arm motion system, a human body three-dimensional model, and the like. Taking a robot as an example, usually, the robot performs a specified action by adjusting the angles of the joints of the arm. The angles and angular velocities of the joints of the arm constitute a description of the specific action. The motion of the robot arm is generally described in a state vector in Cartesian coordinate space (such as velocity, displacement, etc. in a plane coordinate system). For example, for a given set of mechanical arm joint angles, the position and direction of motion of the robotic arm in a Cartesian coordinate system is called a "positive kinematics problem" or "direct kinematics problem" (ie, motion output and joints) The relationship of degrees of freedom). During the movement of the arm, it is necessary to constantly change the rotation angle of each joint to reach the corresponding position point. In order to control the movement of the manipulator, the angle problem between the joints of the corresponding manipulators is solved under the condition of the known trajectory of the manipulator. It is called “inverse kinematics problem” or “reverse kinematics planning problem”. In general, in a robotic arm or motion system with redundancy, the singular state will cause many advantages of the system to be lost. In order to solve this problem, the present invention proposes a method capable of performing inverse kinematic programming online to prevent singular states.

It should be understood that the technical solution of the present invention can be applied to a motion system (for example, a motion system including a robot arm) that involves an inverse kinematics planning problem, which is not limited thereto. Among them, the mechanical arm is essentially a motion system with multiple degrees of freedom.

For example, FIG. 1 shows a structural view of a mechanical arm to which the technical solution of the embodiment of the present invention can be applied. It should be understood that only FIG. 1 is taken as an example here, but does not constitute a limitation of the present invention. As shown in Figure 1, the robotic arm (Mitsubishi PA10-7C) structure mimics the human body structure, including the wrist joint Wrist, the lower arm Lower arm, the elbow joint Elbow, the upper arm Upper arm, and the shoulder Shoulder. The robot arm has seven joint degrees of freedom, and each joint degree of freedom is represented by θ ₁ , θ ₂ , ... θ ₇ , respectively. Where θ ₁ , θ ₃ , θ ₅ are connected by a rotating shaft, and θ ₂ , θ ₄ , and θ ₆ are connected by a pivot. If only the movement of the manipulator is considered, the degree of freedom of movement of the arm is 3, and the degree of redundancy (ie, joint freedom minus motion freedom) is 4, the arm is a redundant manipulator. The positive kinematics of the manipulator is r = f(θ). The positive kinematics expression is a characteristic of the system itself, which is obtained according to the spatial physical properties and motion laws of the mechanical arm.

2 shows a schematic flow diagram of a method 200 of controlling a motion system in accordance with an embodiment of the present invention. The motion system can be the robot arm of Figure 1 above. The method 200 can be performed by the robotic arm described above. As shown in FIG. 2, the method 200 includes:

S210, determining, according to the operability index of the motion system, the singular constraint condition of the motion system, the value of the operability of the motion system preventing the singular constraint condition is greater than 0;

S220: Determine a dynamic optimization model of the motion system according to the singular constraint prevention condition;

S230. Determine a motion parameter of the motion system according to the dynamic optimization model.

S240. Control the motion system according to the motion parameter.

Specifically, the method for controlling a motion system according to an embodiment of the present invention determines a singular constraint condition for preventing a singular constraint condition of the motion system according to an operability of the motion system when performing inverse kinematics planning. The value of the operability of the motion system is greater than 0, and the dynamic optimization model of the motion system is determined according to the singular constraint prevention condition. The dynamic optimization model is solved, and the motion parameter of the motion system can be obtained, and the motion parameter represents the specified pose The angle value between the joints of the corresponding mechanical arm, the motion system controls the motion system to achieve the specified posture according to the motion parameter.

In an embodiment of the invention, the singular constraint is prevented from being used to prevent the singular state from occurring in the motion system. When the value of operability is 0, the motion system produces a singular state. The singularity constraint prevention condition of the embodiment of the present invention is expressed as forcing the value of the operability to be greater than 0, thereby ensuring that the motion system avoids the occurrence of singular states during the motion.

表征，M(θ)表示可操作性的值，det表示矩阵行列式，J(θ)表示雅可比矩阵。这里，当可操作性的值M(θ)＝0时，运动系统产生奇异状态。为了防止运动系统奇异，需要强制该可操作性的值大于0，本发明引入所述防止奇异约束条件来解决该问题。所述防止奇异约束条件具体的实现方法是，对M(θ)按照时间进行求导，当可操作性的值趋于0时，强制其导数为非负值，这样使得可操作数值不再下降，呈上升趋势，从而使得M(θ)＞0，即

进一步地，可以自定义阈值，强制可操作性的值大于该自定义阈值，以防止运动系统发生奇异。例如，该防止奇异约束条件可通过公式

来表征，其中，▽表示梯度算子，

该δ为用户自定义的大于0的预定阈值。In an embodiment of the invention, operability is an indicator of the motion system, similar to energy, speed, and the like. For example, the operability of a motion system can be passed through a formula

Characterization, M(θ) represents the value of operability, det represents the matrix determinant, and J(θ) represents the Jacobian matrix. Here, when the value of operability M(θ) = 0, the motion system generates a singular state. In order to prevent the singularity of the motion system, it is necessary to force the value of the operability to be greater than 0, and the present invention introduces the singularity prevention condition to solve the problem. The specific implementation method for preventing the singular constraint condition is to demodulate M(θ) according to time. When the value of the operability tends to 0, the derivative is forced to be non-negative, so that the operable value is no longer decreased. , showing an upward trend, so that M(θ)>0, ie

Further, the threshold can be customized to force the value of the operability to be greater than the custom threshold to prevent the motion system from being singular. For example, the singularity constraint can be passed through a formula

To characterize, where ▽ denotes a gradient operator,

The δ is a user-defined predetermined threshold greater than zero.

Optionally, the dynamic optimization model may consider a plurality of expected indicators, for example, in the robot path planning, by comprehensively considering joint limits of joint degrees of freedom, avoiding obstacles, avoiding singular states, and optimizing paths, etc. Dynamic optimization model.

Therefore, the method for controlling the motion system according to the embodiment of the present invention determines the singular constraint condition of the motion system according to the operability index of the motion system, and then solves the motion system online according to the dynamic optimization model determined by the singular constraint prevention condition. The motion parameters can avoid the singular state of the motion system, thus enhancing the fault tolerance of the motion system.

It should be understood that, in the embodiment of the present invention, the number of performance indicators of the dynamic optimization model is not limited, and one or more performance indicators may be considered to establish a dynamic optimization model.

For example, optionally, the method 200 further includes:

Determining the objective function according to the energy index and the speed index of the motion system;

Wherein, the dynamic optimization model of the motion system is determined according to the singular constraint prevention condition, including:

The dynamic optimization model of the motion system is determined according to the objective function and the singularity prevention condition.

Specifically, the energy index of the motion system can be the minimum energy indicator, that is, the motion system should complete the given task with as little energy as possible; the speed index can be the minimum joint speed, and the smaller joint speed can make the motion system have more Good maneuverability. Through the energy index and the velocity index, the motion system can determine the objective function, and combined with the prevention of singular constraints, construct a dynamic optimization model of the constrained motion system, that is, the dynamic optimization problem of multi-objective inverse kinematics planning, An optimal solution can be obtained under a plurality of conditions satisfying the minimum energy, the minimum joint speed, and the prevention of singularity, and the control of the motion system is completed according to the optimal solution.

For another example, the method 200 further includes:

Determining a trajectory tracking constraint according to a motion reference trajectory of the motion system;

Determining an obstacle avoidance constraint according to the obstacle avoidance safety distance of the motion system, the obstacle avoidance constraint restricting the motion direction of the motion system away from the obstacle when the distance between the motion system and the obstacle tends to the obstacle avoidance distance The direction of the object;

Wherein, the dynamic optimization model of the motion system is determined according to the singular constraint prevention condition, including:

The dynamic optimization model of the motion system is determined according to the trajectory tracking constraint condition, the obstacle avoidance constraint condition, the objective function, and the singularity prevention condition.

Specifically, the motion system needs to complete a specified task when moving, that is, tracking a specified motion reference trajectory, and determining a trajectory tracking constraint according to the motion reference trajectory. In addition, when there are obstacles in the working space, the motion system should also have the ability to automatically avoid obstacles. The specific performance is to determine the obstacle avoidance constraint according to the obstacle avoidance safety distance of the motion system. The obstacle avoidance constraint is used to constrain the motion direction. When the distance between the motion system and the obstacle tends to avoid the obstacle, the distance is away from the obstacle. Based on the trajectory tracking constraint condition and the obstacle avoidance constraint condition, the motion system combines the above objective function and the singular constraint prevention condition to construct the dynamic optimization model of the constrained motion system, that is, the dynamic optimization of multi-objective inverse kinematics planning. The problem is that an optimal solution can be obtained under a plurality of conditions satisfying minimum energy, minimum joint velocity, tracking motion reference trajectory, avoiding obstacles, and preventing singularity, and completing control of the motion system according to the optimal solution.

Optionally, determining the dynamic optimization model of the motion system according to the trajectory tracking constraint condition, the obstacle avoidance constraint condition, the objective function, and the singularity prevention condition, including:

The dynamic optimization model is determined according to the following formula (1),

为该θ对时间t求导的导数的二范数的平方，该||·||₂表示二范数，该

为该θ对该t求导的导数的无穷范数的平方，该||·||_∞表示无穷范数，该

为该目标函数，该

为该轨迹追踪约束条件，该

为该运动系统的正运动学表达式r＝f(θ)对该t求导的表达式，该J(θ)为该f(θ)的雅可比矩阵，该

为该避障约束条件，该

是由障碍物O指向运动关节C的向量，该J_c(θ)为所述运动关节C对应的雅可比矩阵，该

为该防止奇异约束条件， 该

该δ为大于0的预定阈值，该

该det表示矩阵行列式，该

为该θ的上限θ_max对该t求导的导数，该

为该θ的下限θ_min对该t求导的导数。这里，

可以表示能量最小，

可以表示速度最小。Wherein, the motion system is a motion system with n joint degrees of freedom θ=(θ ₁ , θ ₂ , . . . , θ _n ),

The square of the norm of the derivative of which θ is derived from time t, the ||·|| ₂ represents a two norm,

For the square of the infinite norm of the derivative of the θ derived from t, the ||·|| _∞ represents an infinite norm,

For the objective function, the

Tracking constraints for the trajectory,

An expression for which t is derived for the positive kinematic expression r=f(θ) of the motion system, the J(θ) being the Jacobian matrix of the f(θ),

For the obstacle avoidance constraint,

Is a vector pointing from the obstacle O to the moving joint C, and the J _c (θ) is a Jacobian matrix corresponding to the moving joint C,

For this to prevent singular constraints,

The δ is a predetermined threshold greater than 0, the

The det represents a matrix determinant,

Deriving the derivative of t for the upper limit θ _{max of} θ,

The derivative of the derivative of t is the lower limit θ _{min of} θ. Here,

Can indicate that energy is minimal,

It can mean that the speed is the smallest.

In the embodiment of the present invention, a continuous time dynamic optimization model is constructed to represent the inverse kinematic programming problem with redundancy. The inverse kinematic programming problem uses the derivative of the joint degree of freedom (ie, the velocity variable) as the decision variable, and the sum of the two norms and the infinite norm of the derivative of the joint degree of freedom is the objective function, and the trajectory tracking is the equality constraint. The obstacle avoidance and the prevention of singularity are inequality constraints, so that the optimal solution, that is, the motion parameters obtained from the inverse kinematics planning problem, realizes the control of the motion system.

其中，

该det表示矩阵行列式，该δ＞0，为自定义的阈值。In the embodiment of the present invention, a representation method for preventing singular degrees of freedom is proposed for the first time, that is,

among them,

The det represents a matrix determinant, and δ>0, which is a custom threshold.

Optionally, as an embodiment, determining the motion parameter of the motion system according to the dynamic optimization model includes:

The motion parameter of the motion system is determined according to the neural network model corresponding to the dynamic optimization model.

Specifically, after obtaining the dynamic optimization model, the dynamic optimization problem needs to be solved, and the dynamic optimization model is solved online by using the neural network to obtain the motion parameters of the motion system. The neural network is introduced online to avoid complex parameterization and derivation processes. Moreover, the neural network model has a simple structure and does not require manual adjustment of internal parameters.

Optionally, determining the motion parameter of the motion system according to the neural network model corresponding to the dynamic optimization model, including:

The motion parameter is determined according to the state equation of the neural network model of the following formula (2),

该J(θ)^T为该J(θ)的转置矩阵，该A为

该A^T为该A的转置矩阵，该K为n×n矩阵，且该K矩阵的第k行第k列元素为1其余元素均为0，该b为

Wherein the μ and the ρ are state parameters of the state equation, the ε is a gain, and the g is

The J(θ) ^{T is} a transposed matrix of the J(θ), and the A is

The A ^{T is} a transposed matrix of the A, the K is an n×n matrix, and the kth row and the kth column element of the K matrix are 1 and the remaining elements are all 0, and the b is

得到式(2)即为神经网络的状态方程，其中，式(2)中的ε为一个足够大的增益，该g为

的激励函数，

为n维的向量，假设

是n维向量

中绝对值数值最大的元素，即

定义K为n×n矩阵，且该K矩阵的第k行第k列元素为1其余元素均为0。Specifically, for the dynamic optimization model (1), a neural network model can be designed to calculate the global optimal solution of the equation (1) in real time. Defined according to formula (1)

The equation (2) is obtained as the state equation of the neural network, wherein ε in the equation (2) is a sufficiently large gain, and the g is

Incentive function,

As an n-dimensional vector, hypothesis

Is an n-dimensional vector

The element with the highest absolute value, ie

The definition K is an n×n matrix, and the kth row and the kth column element of the K matrix are 1 and the remaining elements are all 0.

以及关节自由度θ，将它们反馈给运动系统，使得运动系统按照最优的关节速度或关节自由度进行操作。Figure 3 shows a schematic flow chart of a neural network based inverse kinematics plan. It should be noted that this is only to assist those skilled in the art to better understand the embodiments of the present invention and not to limit the scope of the embodiments of the present invention. As shown in FIG. 3, for a motion system with redundancy, a corresponding Jacobian matrix is obtained according to its known positive kinematics expression, and multiple singularities, desired poses, obstacle avoidance indicators, etc. of the motion system are considered. The expected index is obtained by multi-objective inverse motion planning description, that is, the dynamic optimization model. The dynamic optimization model is substituted into the neural network model, and the state equation corresponding to the neural network model is solved to obtain the optimal joint speed.

And joint freedom degrees θ, which are fed back to the motion system so that the motion system operates at the optimal joint speed or joint freedom.

Here, the embodiment of the invention uses the neural network to optimize the inverse kinematic programming problem online, avoids the complicated parameterized transportation and derivation process, and can output the optimal degree of freedom velocity information of the inverse kinematic programming problem in real time through the neural network. The modeling process of the neural network model can be referred to the prior art, and will not be described here.

It should be understood that the state equation of the neural network describes the model and learning rules of the neural network, which can be understood as a functional module, and its physical implementation may be a neuromorphic chip, a digital signal processor (Digital Signal Processor, referred to as "DSP"). ), software simulation, etc., the present invention Not limited.

Therefore, the method for controlling the motion system according to the embodiment of the present invention determines the singularity constraint condition of the motion system according to the operability index of the motion system, and then determines the dynamic optimization model according to the singular constraint prevention condition and other multiple performance indicators. Further, the motion parameters of the motion system are solved online by the neural network, and offline training is not needed, which can avoid the singular state of the motion system, thereby enhancing the fault tolerance of the motion system.

The implementation of the method of controlling the motion system of the present invention will be described below by taking the motion system of the robot arm of FIG. 1 as an example.

机械臂的末端操作器要在操作空间完成一个半径为20厘米的圆周操作，并且要躲避操作空间内的两个障碍物，即机械臂的任意部位距离任何一个障碍物都要求在5厘米之外。两个障碍物的空间位置坐标分别是(0,-0.2,1)和(-0.1,0.1,0.8)。并且，在运动过程中，约束机械臂的各个关节自由度的速度

的绝对值都必须小于1rad/s。其中，上述半径为20厘米的圆周操作的运动参考轨迹如下式(3)所示：Assume that the initial state of the seven joint degrees of freedom of the robot arm in Fig. 1 is

The end manipulator of the manipulator is to perform a circular operation with a radius of 20 cm in the operating space, and to avoid two obstacles in the operating space, that is, any part of the manipulator is required to be 5 cm away from any obstacle. . The spatial position coordinates of the two obstacles are (0, -0.2, 1) and (-0.1, 0.1, 0.8), respectively. And, during the movement, the speed of each joint degree of freedom of the arm is constrained

The absolute value must be less than 1 rad/s. Wherein, the motion reference trajectory of the circumferential operation with the above radius of 20 cm is as shown in the following formula (3):

Wherein, x ₀ , y ₀ , and z ₀ are initial states of the robot arm in a Cartesian coordinate system. In the method of controlling the motion system of the present invention, the degrees of freedom of the joints of the above-mentioned robot arms are solved, and the specific implementation is as follows:

描述的运动学表达式

其中J(θ)为f(θ)的雅可比矩；1) According to the positive kinematics expression r=f(θ) of the manipulator, the derivative obtains the velocity information.

Descriptive kinematic expression

Where J(θ) is the Jacobian moment of f(θ);

描述该机械臂在逆运动学规划中期望的多个指标：2) Adopt speed information

Describe the various indicators that the robotic arm expects in the inverse kinematics plan:

表示，关节速度指标用

表示；轨迹追踪用

表示，对于本例而言，对上式(3)求导，得到的轨迹追踪约束条件的具体形式如下所示： Energy indicator

Said that the joint speed indicator is used

Representation;

It is shown that, for this example, the specific form of the trajectory tracking constraint obtained by deriving the above equation (3) is as follows:

表示，这里，OC＝(OC₁；OC₂)，OC₁是由第一个障碍物(0,-0.2,1)到机械臂的投影向量计算获得，OC₂第二个障碍物(-0.1,0.1,0.8)到机械臂的投影向量计算获得，J_c(θ)的符号表达式与J(θ)相同；防止奇异约束条件采用

表示，其中

det表示矩阵行列式；Obstacle avoidance conditions for avoiding obstacles

Here, OC = (OC ₁ ; OC ₂ ), OC ₁ is calculated from the projection vector of the first obstacle (0, -0.2, 1) to the arm, and the second obstacle of OC ₂ (-0.1 , 0.1, 0.8) is obtained by calculating the projection vector of the manipulator. The symbolic expression of J _c (θ) is the same as J(θ); preventing the use of singular constraints

Said that

Det represents a matrix determinant;

3) Determine the dynamic optimization model based on the above multiple expected indicators:

为该θ的上限，该

为该θ的下限θ_min，这里

为-1rad/s，

为1rad/s。Among them, the

For the upper limit of θ, the

For the lower limit θ _{min of} θ , here

Is -1rad/s,

It is 1 rad/s.

4) Substituting the dynamic optimization model in the above 3) and the initial value of θ into the state equation of the neural network model, running the neural network to obtain the output of the neural network, that is, the optimal degree of freedom of each joint of the robot at the sampling moment Numerical Solution.

The neural network-based inverse kinematics planning method of the manipulator in this example is given in 1) to 4) above. The method can be verified by simulation. For example, it can be used in VC6.0 (ie Microsoft Visual C++ 6.0) environment. In the Matlab environment, programming simulation is used to implement the inverse kinematics planning method, which is not limited.

The simulation results of the online inverse kinematics planning of the robot arm in this example will be described below with reference to FIGS. 4 to 6. It should be noted that this is only to assist those skilled in the art to better understand the embodiments of the present invention and not to limit the scope of the embodiments of the present invention.

Figure 4 shows a simulation result of the motion trajectory of the robot arm. As shown in FIG. 4, the three axes of the operating space of the arm movement path are the x-axis (m), the y-axis (m), and the z-axis (m), respectively. In the Cartesian coordinate system, the two black points correspond to the spatial coordinates (0, -0.2, 1) and (-0.1, 0.1, 0.8) of the obstacle, respectively. Simulation results It shows that the robot arm can complete the required circular operation with a radius of 20 cm, and there is no collision obstacle during the movement, and no singular state is generated. Here, the generation of singularity can be understood as the fact that the two rods coincide in a straight line, that is, one degree of freedom is lost.

的绝对值在任意时刻均控制在1rad/s内。其中，有个关节不需要移动，即其输出值与x轴重合。Fig. 5 is a graph showing the simulation results of the velocity information of the joint degrees of freedom of the robot arm. As shown in Fig. 5, the horizontal axis t is time (s) and the longitudinal axis joint velocity (rad/s), and the seven lines in the figure represent velocity information of seven joint degrees of freedom, respectively. The simulation results show that the speed of the seven joint degrees of freedom during the movement

The absolute value is controlled at 1 rad/s at any time. Among them, there is a joint that does not need to move, that is, its output value coincides with the x-axis.

Fig. 6 is a graph showing the simulation results of the manipulator's operability index. As shown in FIG. 6, the horizontal axis t is time (s), and the vertical axis M represents an operability index, that is, the constraint-preventing singular condition of the embodiment of the present invention, and the simulation result 1 in the figure is the operability index of the present invention. The simulation result 2 is an operability index which does not employ the technical solution of the present invention. The simulation results show that if the technical solution of the present invention is not adopted, in the 4th to 6s motion interval, the simulation result 2 has a singular state, and the value of the operability index tends to 0, and the technical solution of the present invention is used for simulation. As a result, the singular state does not occur, thereby embodying the superiority of the technical solution of the present invention.

In the embodiment of the present invention, the mechanical arm determines a dynamic optimization model according to a plurality of performance indicators, the performance indicators include minimum energy, minimum speed, trajectory tracking, prevention of singularity, and avoidance of obstacles. Further, the dynamic optimization is solved online by using a neural network. The model obtains the optimal numerical solution of the seven joint degrees of freedom of the robot arm, eliminating the need for off-line training, and avoiding the singular state of the robot arm, thereby enhancing the fault tolerance of the system.

It should be understood that, in various embodiments of the present invention, the size of the sequence numbers of the above processes does not mean the order of execution, and the order of execution of each process should be determined by its function and internal logic, and should not be directed to the embodiments of the present invention. The implementation process constitutes any limitation.

A method of controlling a motion system according to an embodiment of the present invention is described in detail above, and an apparatus for controlling a motion system according to an embodiment of the present invention will be described below.

FIG. 7 shows a schematic block diagram of an apparatus 700 for controlling a motion system in accordance with an embodiment of the present invention. As shown in FIG. 7, the apparatus 700 includes:

a determining module 710, configured to determine, according to the operability index of the motion system, a singular constraint condition of the motion system, the singular constraint condition limiting the operability of the motion system is greater than 0;

The optimization module 720 is configured to determine, according to the determining module 710, the singularity constraint Determining the dynamic optimization model of the motion system;

The processing module 730 is configured to determine a motion parameter of the motion system according to the dynamic optimization model determined by the optimization module 720;

The control module 740 is configured to control the motion system according to the motion parameter determined by the processing module 730.

The method for controlling a motion system according to an embodiment of the present invention determines a singular constraint condition of the motion system according to an operability index of the motion system, and then dynamically calculates the motion of the motion system according to the dynamic optimization model determined by the singular constraint prevention condition. The parameters can avoid the singular state of the motion system, thus enhancing the fault tolerance of the motion system.

In the embodiment of the present invention, the determining module 710 is further configured to determine an objective function according to the energy indicator and the speed indicator of the motion system;

The optimization module 720 is specifically configured to:

The dynamic optimization model of the motion system is determined according to the objective function determined by the determining module 710 and the singularity prevention condition.

In the embodiment of the present invention, the determining module 710 is further configured to determine a trajectory tracking constraint according to the motion reference trajectory of the motion system;

The determining module 710 is further configured to determine an obstacle avoidance constraint according to the obstacle avoidance safety distance of the motion system, the obstacle avoidance constraint restricting a motion direction of the motion system, and a distance between the motion system and the obstacle tends to avoid the obstacle The safety distance is the direction away from the obstacle;

The optimization module 720 is specifically configured to:

The dynamic optimization model of the motion system is determined according to the trajectory tracking constraint condition, the obstacle avoidance constraint condition, the objective function, and the singularity prevention condition.

In the embodiment of the present invention, the optimization module 720 is specifically configured to:

Determine the dynamic optimization model according to the following formula,

为该θ对时间t求导的导数的二范数的平方，该||·||₂表示二范数，该

为该 θ对该t求导的导数的无穷范数的平方，该||·||_∞表示无穷范数，该

为该目标函数，该

为该轨迹追踪约束条件，该

为该运动系统的正运动学表达式r＝f(θ)对该t求导的表达式，该J(θ)为该f(θ)的雅可比矩阵，该

为该避障约束条件，该

是由障碍物O指向运动关节C的向量，该J_c(θ)为所述运动关节C对应的雅可比矩阵，该

为该防止奇异约束条件，该

该δ为大于0的预定阈值，该

该det表示矩阵行列式，该

为该θ的上限θ_max对该t求导的导数，该

为该θ的下限θ_min对该t求导的导数。Wherein, the motion system is a motion system with n joint degrees of freedom θ=(θ ₁ , θ ₂ , . . . , θ _n ),

The square of the norm of the derivative of which θ is derived from time t, the ||·|| ₂ represents a two norm,

For the square of the infinite norm of the derivative of which θ is derived for t, the ||·|| _∞ represents an infinite norm,

For the objective function, the

Tracking constraints for the trajectory,

An expression for which t is derived for the positive kinematic expression r=f(θ) of the motion system, the J(θ) being the Jacobian matrix of the f(θ),

For the obstacle avoidance constraint,

Is a vector pointing from the obstacle O to the moving joint C, and the J _c (θ) is a Jacobian matrix corresponding to the moving joint C,

For this to prevent singular constraints,

The δ is a predetermined threshold greater than 0, the

The det represents a matrix determinant,

Deriving the derivative of t for the upper limit θ _{max of} θ,

The derivative of the derivative of t is the lower limit θ _{min of} θ.

In the embodiment of the present invention, the processing module 730 is specifically configured to:

The motion parameter of the motion system is determined according to the neural network model corresponding to the dynamic optimization model.

In the embodiment of the present invention, the processing module 730 is specifically configured to:

The motion parameter is determined according to the following state equation of the neural network model,

该J(θ)^T为该J(θ)的转置矩阵，该A为

该A^T为该A的转置矩阵，该K为n×n矩阵，且该K矩阵的第k行第k列元素为1其余元素均为0，该b为

Wherein the μ and the ρ are state parameters of the state equation, the ε is a gain, and the g is

The J(θ) ^{T is} a transposed matrix of the J(θ), and the A is

The A ^{T is} a transposed matrix of the A, the K is an n×n matrix, and the kth row and the kth column element of the K matrix are 1 and the remaining elements are all 0, and the b is

Therefore, the device for controlling the motion system according to the embodiment of the present invention determines the singular constraint condition of the motion system according to the operability index of the motion system, and then determines the dynamic optimization model according to the singular constraint prevention condition and other multiple performance indicators. Further, the motion parameters of the motion system are solved online by using a neural network, and offline training is not required, and the singularity of the motion system can be avoided. State, thus enhancing the fault tolerance of the motion system.

Apparatus 700 for controlling a motion system in accordance with an embodiment of the present invention may perform a method of controlling a motion system in accordance with an embodiment of the present invention, and the above and other operations and/or functions of the various modules in the apparatus 700 are respectively implemented to implement the various methods described above. The corresponding process, for the sake of brevity, will not be described here.

FIG. 8 shows a structure of an apparatus for controlling a motion system according to still another embodiment of the present invention, comprising at least one processor 802 (for example, a CPU), at least one network interface 805 or other communication interface, a memory 806, and at least one communication. A bus 803 is used to implement connection communication between these devices. The processor 802 is configured to execute executable modules, such as computer programs, stored in the memory 806. The memory 806 may include a high speed random access memory (RAM: Random Access Memory), and may also include a non-volatile memory such as at least one disk memory. A communication connection with at least one other network element is achieved by at least one network interface 805 (which may be wired or wireless).

In some embodiments, the memory 806 stores a program 8061, and the processor 802 executes the program 8061 for performing the method of controlling the motion system of the aforementioned embodiment of the present invention.

It should be understood that the term "and/or" herein is merely an association relationship describing an associated object, indicating that there may be three relationships, for example, A and/or B, which may indicate that A exists separately, and A and B exist simultaneously. There are three cases of B alone. In addition, the character "/" in this article generally indicates that the contextual object is an "or" relationship.

It should be understood that, in various embodiments of the present invention, the size of the sequence numbers of the above processes does not mean the order of execution, and the order of execution of each process should be determined by its function and internal logic, and should not be directed to the embodiments of the present invention. The implementation process constitutes any limitation.

Those of ordinary skill in the art will appreciate that the elements and algorithm steps of the various examples described in connection with the embodiments disclosed herein can be implemented in electronic hardware or a combination of computer software and electronic hardware. Whether these functions are performed in hardware or software depends on the specific application and design constraints of the solution. A person skilled in the art can use different methods for implementing the described functions for each particular application, but such implementation should not be considered to be beyond the scope of the present invention.

A person skilled in the art can clearly understand that for the convenience and brevity of the description, the specific working process of the system, the device and the unit described above can refer to the corresponding process in the foregoing method embodiment, and details are not described herein again.

In the several embodiments provided by the present application, it should be understood that the disclosed systems, devices, and methods may be implemented in other manners. For example, the device embodiments described above are merely illustrative For example, the division of the unit is only a logical function division, and the actual implementation may have another division manner, for example, multiple units or components may be combined or may be integrated into another system, or some features may be Ignore, or not execute. In addition, the mutual coupling or direct coupling or communication connection shown or discussed may be an indirect coupling or communication connection through some interface, device or unit, and may be in an electrical, mechanical or other form.

The units described as separate components may or may not be physically separated, and the components displayed as units may or may not be physical units, that is, may be located in one place, or may be distributed to multiple network units. Some or all of the units may be selected according to actual needs to achieve the purpose of the solution of the embodiment.

In addition, each functional unit in each embodiment of the present invention may be integrated into one processing unit, or each unit may exist physically separately, or two or more units may be integrated into one unit.

The functions may be stored in a computer readable storage medium if implemented in the form of a software functional unit and sold or used as a standalone product. Based on such understanding, the technical solution of the present invention, which is essential or contributes to the prior art, or a part of the technical solution, may be embodied in the form of a software product, which is stored in a storage medium, including The instructions are used to cause a computer device (which may be a personal computer, server, or network device, etc.) to perform all or part of the steps of the methods described in various embodiments of the present invention. The foregoing storage medium includes: a U disk, a mobile hard disk, a read-only memory (ROM), a random access memory (RAM), a magnetic disk, or an optical disk, and the like. .

The above is only a specific embodiment of the present invention, but the scope of the present invention is not limited thereto, and any person skilled in the art can easily think of changes or substitutions within the technical scope of the present invention. It should be covered by the scope of the present invention. Therefore, the scope of the invention should be determined by the scope of the appended claims.

Claims (13)

A method of controlling a motion system, comprising: Determining a singular constraint condition of the motion system according to an operability index of the motion system, the value of the operability preventing the singular constraint condition binding the motion system being greater than 0; Determining a dynamic optimization model of the motion system according to the preventing singular constraint condition; Determining motion parameters of the motion system according to the dynamic optimization model; The motion system is controlled in accordance with the motion parameters. The method of claim 1 further comprising: Determining an objective function according to an energy index and a speed index of the motion system; Wherein, the determining a dynamic optimization model of the motion system according to the preventing singular constraint condition comprises: The dynamic optimization model of the motion system is determined according to the objective function and the singularity prevention condition. The method of claim 2, wherein the method further comprises: Determining a trajectory tracking constraint according to a motion reference trajectory of the motion system; Determining an obstacle avoidance constraint according to an obstacle avoidance safety distance of the motion system, the obstacle avoidance constraint restricting a motion direction of the motion system, and a distance between the motion system and the obstacle tends to the obstacle avoidance safety The distance is away from the obstacle; Wherein, the determining a dynamic optimization model of the motion system according to the preventing singular constraint condition comprises: Determining the dynamic optimization model of the motion system according to the trajectory tracking constraint, the obstacle avoidance constraint, the objective function, and the singularity prevention condition. The method of claim 3, wherein said determining said said motion system based on said trajectory tracking constraint, said obstacle avoidance constraint, said objective function, and said singularity prevention condition Dynamic optimization models, including: The dynamic optimization model is determined according to the following formula,

Wherein the motion system is a motion system with n joint degrees of freedom θ=(θ ₁ , θ ₂ , . . . , θ _n ),

a square of a norm of a derivative of the derivative of θ versus time t, the ||·|| ₂ representing a two norm,

a square of an infinite norm of a derivative of the derivative of θ for the t, the ||·|| _∞ representing an infinite norm,

For the objective function, the

Tracking constraints for the trajectory,

An expression that is derived for the t from the positive kinematics expression r=f(θ) of the motion system, the J(θ) being the Jacobian matrix of the f(θ),

For the obstacle avoidance constraint, the

Is a vector directed by the obstacle O to the moving joint C, the J _c (θ) being a Jacobian matrix corresponding to the moving joint C,

For the prevention of singular constraints, the

The δ is a predetermined threshold greater than 0,

The det represents a matrix determinant,

Deriving the derivative of the t by the upper limit θ _max of the θ,

The derivative of the t is derived for the lower limit θ _min of the θ. The method according to claim 4, wherein the determining the motion parameters of the motion system according to the dynamic optimization model comprises: Determining the motion parameter of the motion system according to a neural network model corresponding to the dynamic optimization model. The method according to claim 5, wherein the determining the motion parameter of the motion system according to the neural network model corresponding to the dynamic optimization model comprises: The motion parameters are determined according to a state equation of a neural network model described below,

Wherein the μ and the ρ are state parameters of the state equation, the ε is a gain, and the g is

The J(θ) ^T is a transposed matrix of the J(θ), and the A is

The A ^T is a transposed matrix of the A, the K is an n×n matrix, and the kth row and the kth column element of the K matrix are 1 and the remaining elements are all 0, and the b is

A device for controlling a motion system, comprising: a determining module, configured to determine a singular constraint condition of the motion system according to an operability index of the motion system, the value of preventing the singular constraint condition from constraining the operability of the motion system to be greater than 0; And an optimization module, configured to determine a dynamic optimization model of the motion system according to the singularity prevention condition determined by the determining module; a processing module, configured to determine a motion parameter of the motion system according to the dynamic optimization model determined by the optimization module; And a control module, configured to control the motion system according to the motion parameter. The apparatus according to claim 7, wherein the determining module is further configured to determine an objective function according to an energy indicator and a speed indicator of the motion system; The optimization module is specifically configured to: Determining the dynamic optimization model of the motion system according to the objective function determined by the determining module and the singularity prevention condition. The apparatus according to claim 8, wherein the determining module is further configured to determine a trajectory tracking constraint according to a motion reference trajectory of the motion system; The determining module is further configured to determine an obstacle avoidance constraint according to an obstacle avoidance safety distance of the motion system, where the obstacle avoidance constraint restricts a motion direction of the motion system at a distance between the motion system and the obstacle Approaching the safe distance of the obstacle avoidance is away from the obstacle; The optimization module is specifically configured to: Determining the dynamic optimization model of the motion system according to the trajectory tracking constraint, the obstacle avoidance constraint, the objective function, and the singularity prevention condition. The device according to claim 9, wherein the optimization module is specifically configured to: The dynamic optimization model is determined according to the following formula,

Wherein the motion system is a motion system with n joint degrees of freedom θ=(θ ₁ , θ ₂ , . . . , θ _n ),

a square of a norm of a derivative of the derivative of θ versus time t, the ||·|| ₂ representing a two norm,

a square of an infinite norm of a derivative of the derivative of θ for the t, the ||·|| _∞ representing an infinite norm,

For the objective function, the

Tracking constraints for the trajectory,

An expression that is derived for the t from the positive kinematics expression r=f(θ) of the motion system, the J(θ) being the Jacobian matrix of the f(θ),

For the obstacle avoidance constraint, the

Is a vector pointing from the obstacle O to the moving joint C,

For the prevention of singular constraints, the

The δ is a predetermined threshold greater than 0,

The det represents a matrix determinant,

Deriving the derivative of the t by the upper limit θ _max of the θ,

The derivative of the t is derived for the lower limit θ _min of the θ. The device according to claim 10, wherein the processing module is specifically configured to: Determining the motion parameter of the motion system according to a neural network model corresponding to the dynamic optimization model. The device according to claim 11, wherein the processing module is specifically configured to: The motion parameters are determined according to a state equation of a neural network model described below,

Wherein the μ and the ρ are state parameters of the state equation, the ε is a gain, and the g is

The J(θ) ^T is a transposed matrix of the J(θ), and the A is

The A ^T is a transposed matrix of the A, the K is an n×n matrix, and the kth row and the kth column element of the K matrix are 1 and the remaining elements are all 0, and the b is

A device for controlling a motion system, comprising: a processor, a memory, and a pass Letter interface The processor is coupled to the memory and the communication interface, the memory is for storing instructions, the processor is configured to execute the instruction, and the communication interface is used under the control of the processor and other networks The unit is in communication, and when the apparatus is in operation, the processor executes the instructions stored in the memory to cause the apparatus to perform the method of any one of claims 1 to 6.

Images