CN111571586B - Dynamic constraint method for inverse dynamics freedom degree - Google Patents
Dynamic constraint method for inverse dynamics freedom degree Download PDFInfo
- Publication number
- CN111571586B CN111571586B CN202010379166.4A CN202010379166A CN111571586B CN 111571586 B CN111571586 B CN 111571586B CN 202010379166 A CN202010379166 A CN 202010379166A CN 111571586 B CN111571586 B CN 111571586B
- Authority
- CN
- China
- Prior art keywords
- freedom
- degree
- constraint
- joint
- matrix
- 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
Images
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1602—Programme controls characterised by the control system, structure, architecture
- B25J9/1607—Calculation of inertia, jacobian matrixes and inverses
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- Mathematical Physics (AREA)
- Automation & Control Theory (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Processing Or Creating Images (AREA)
Abstract
The invention provides a dynamic constraint method of inverse dynamics freedom degree, which comprises the steps of S1, setting the freedom degree constraint of each joint; s2, constructing a Jacobian matrix of a skeleton level through each joint, and when one degree of freedom of one joint is restrained, erasing elements of the degree of freedom on a corresponding column of the Jacobian matrix, and enabling subsequent elements to shrink forwards; s3, setting a position matrix of inverse dynamics of each joint; s4, substituting the position matrix and the Jacobian matrix into a reverse dynamics numerical solution to obtain an angle rotation result; s5, selecting angle values from the angle rotation results through the freedom degree constraint of each joint to update the freedom degree constraint of all joints; s6, judging whether the degree of freedom constraint of each current joint meets the iteration termination condition of inverse dynamics calculation; in summary, when the degree of freedom constraint is calculated, not only the calculation amount is not increased, but also the calculation is accelerated, and the degree of freedom constraint setting can be changed in the calculation process.
Description
Technical Field
The invention belongs to the technical field of freedom degree constraint, and particularly relates to a dynamic constraint method for a reverse dynamics freedom degree.
Background
In using inverse kinematics, it is often the case that a joint (such as an elbow) that only needs to rotate along a particular axis is required. This situation can be treated with an angle hard constraint solution as is conventional. But the angle hard constraint is very costly and can severely slow down the computation speed. Especially under certain bone structures, excessive hard constraints may increase computational expense by more than ten times.
For example, most of the time the inverse kinematics only need to compute one axis, a robotic arm, a human arm or leg inverse kinematics computation, etc. The constraint scheme is more laborious if dynamic construction is added to the degree of freedom constraint, such as changing the bone rotation axis when the rotation reaches a certain position. There is a need for a more rapid and convenient way to deal with the constraint problem of a single dynamically growing axial degree of freedom.
Disclosure of Invention
The invention aims to provide a dynamic constraint method of inverse dynamics freedom degree, which solves the problem of huge calculation consumption caused by processing dynamic constraint by adding hard constraint in the prior art.
The invention provides the following technical scheme:
a dynamic constraint method of inverse dynamic freedom degrees comprises the following steps:
s1, setting the freedom degree constraint of each joint, and further determining the freedom degree required to be calculated by each joint;
s2, constructing a Jacobian matrix of a skeleton level through each joint, and when one degree of freedom of one joint is restrained, erasing elements of the degree of freedom on a corresponding column of the Jacobian matrix, and enabling subsequent elements to shrink forwards;
s3, setting a position matrix of inverse dynamics of each joint;
s4, substituting the position matrix and the Jacobian matrix into a reverse dynamics numerical solution to obtain an angle rotation result;
s5, selecting angle values from the angle rotation results through the freedom degree constraint of each joint to update the freedom degree constraint of all joints;
s6, judging whether the degree of freedom constraint of each joint at present meets the iteration termination condition of inverse dynamics calculation,
if yes, the updated freedom degree constraints of all the joints are the calculation result;
if not, the process returns to S1.
Preferably, when it is desired to modify the degree of freedom constraint of one joint, the modification of the degree of freedom constraint may be performed when S6 jumps to S1.
Preferably, in S2, the elements in the jacobian matrix are shrunk forward, so that the result of the angular rotation in S4 is also shrunk accordingly, and finally, the shrunk elements are in one-to-one correspondence with the degree-of-freedom constraint.
Preferably, the setting of the position matrix of the inverse kinematics of each joint comprises determining one or more target positions, performing rotational transformation on each bone chain to obtain target positions of the terminal bones, and constructing the position matrix according to the target positions.
The invention has the beneficial effects that:
the dynamic constraint method of the inverse dynamics degree of freedom is achieved by modifying the inverse dynamics matrix, is irrelevant to a specific inverse dynamics algorithm, does not need the cooperation of a specific inverse dynamics algorithm, and occurs in the construction stage of the operation matrix, so the method can be used under various conditions. Meanwhile, the dynamic constraint method achieves the purpose of constraint by contracting the operation matrix, so that when the degree of freedom constraint is more, the matrix required to be calculated is smaller, and the calculation speed is greatly improved. When special computational operations need to be satisfied, such as when a bone moves to a certain position, modifying the degree of freedom constraints or changing the degrees of freedom of the constraints. The tool can dynamically modify the degree-of-freedom constraint without modifying the calculation conditions and adding a large number of matrix constraint settings.
Drawings
The accompanying drawings, which are included to provide a further understanding of the invention and are incorporated in and constitute a part of this specification, illustrate embodiments of the invention and together with the description serve to explain the principles of the invention and not to limit the invention. In the drawings:
FIG. 1 is a schematic flow diagram of the process of the present invention.
Detailed Description
As shown in fig. 1, a dynamic constraint method for inverse kinematic degrees of freedom includes the following steps:
s1, setting the freedom degree constraint of each joint, and further determining the freedom degree required to be calculated by each joint;
s2, constructing a Jacobian matrix of a skeleton level through each joint, and when one degree of freedom of one joint is restrained, erasing elements of the degree of freedom on a corresponding column of the Jacobian matrix, and enabling subsequent elements to shrink forwards;
s3, setting a position matrix of inverse dynamics of each joint; determining one or more target positions, performing rotation transformation on each skeleton chain to obtain the target position of the terminal skeleton, and constructing a position matrix according to the target position;
s4, substituting the position matrix and the Jacobian matrix into a reverse dynamics numerical solution to obtain an angle rotation result;
s5, selecting angle values from the angle rotation results through the freedom degree constraint of each joint to update the freedom degree constraint of all joints;
s6, judging whether the degree of freedom constraint of each joint at present meets the iteration termination condition of inverse dynamics calculation,
if yes, the updated freedom degree constraints of all the joints are the calculation result;
if not, the process returns to S1.
Specifically, when the degree of freedom constraint of one joint needs to be modified, the degree of freedom constraint can be modified when S6 jumps to S1; in S2, the elements in the jacobian matrix shrink forward, so that the angular rotation result in S4 also shrinks correspondingly, and finally the shrunk elements correspond to the degree-of-freedom constraints one by one.
The running code is as follows:
although the present invention has been described in detail with reference to the foregoing embodiments, it will be apparent to those skilled in the art that changes may be made in the embodiments and/or equivalents thereof without departing from the spirit and scope of the invention. Any modification, equivalent replacement, or improvement made within the spirit and principle of the present invention should be included in the protection scope of the present invention.
Claims (4)
1. A dynamic constraint method of inverse dynamic degrees of freedom is characterized by comprising the following steps:
s1, setting the freedom degree constraint of each joint, and further determining the freedom degree required to be calculated by each joint;
s2, constructing a Jacobian matrix of a skeleton level through each joint, and when one degree of freedom of one joint is restrained, erasing elements of the degree of freedom on a corresponding column of the Jacobian matrix, and enabling subsequent elements to shrink forwards;
s3, setting a position matrix of inverse dynamics of each joint;
s4, substituting the position matrix and the Jacobian matrix into a reverse dynamics numerical solution to obtain an angle rotation result;
s5, selecting angle values from the angle rotation results through the freedom degree constraint of each joint to update the freedom degree constraint of all joints;
s6, judging whether the degree of freedom constraint of each joint at present meets the iteration termination condition of inverse dynamics calculation,
if yes, the updated freedom degree constraints of all the joints are the calculation result;
if not, the process returns to S1.
2. The method of claim 1, wherein when the degree of freedom constraint of a joint needs to be modified, the degree of freedom constraint is modified at S6 jump S1.
3. The method of claim 1, wherein in S2, the elements in the jacobian matrix are shrunk forward, so that the result of the angular rotation in S4 is correspondingly shrunk, and finally, the shrunk elements are in one-to-one correspondence with the degree-of-freedom constraints.
4. The method of claim 1, wherein the setting of the inverse kinematics position matrix for each joint comprises determining one or more target positions, performing a rotational transformation on each bone chain to obtain a target position of the terminal bone, and constructing the position matrix according to the target position.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010379166.4A CN111571586B (en) | 2020-05-07 | 2020-05-07 | Dynamic constraint method for inverse dynamics freedom degree |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010379166.4A CN111571586B (en) | 2020-05-07 | 2020-05-07 | Dynamic constraint method for inverse dynamics freedom degree |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN111571586A CN111571586A (en) | 2020-08-25 |
| CN111571586B true CN111571586B (en) | 2020-12-11 |
Family
ID=72120794
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202010379166.4A Active CN111571586B (en) | 2020-05-07 | 2020-05-07 | Dynamic constraint method for inverse dynamics freedom degree |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN111571586B (en) |
Family Cites Families (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101612734B (en) * | 2009-08-07 | 2011-01-26 | 清华大学 | Pipeline spraying robot and operation track planning method thereof |
| EP3387496A4 (en) * | 2015-12-11 | 2019-07-31 | ABB Schweiz AG | Robot off-line programming method and appartus using the same |
| CN106873621B (en) * | 2017-02-26 | 2020-02-21 | 南京航空航天大学 | Unmanned aerial vehicle formation control algorithm based on Lagrange equation |
| CN108876815B (en) * | 2018-04-28 | 2021-03-30 | 深圳市瑞立视多媒体科技有限公司 | Skeleton posture calculation method, character virtual model driving method and storage medium |
| CN109955250A (en) * | 2019-01-21 | 2019-07-02 | 中国船舶重工集团公司第七一六研究所 | Tracking applied to industrial robot reacts planning algorithm with Real Time Obstacle Avoiding |
| CN110421556B (en) * | 2019-06-14 | 2022-07-19 | 河北工业大学 | Track planning method for real-time collision avoidance of redundant double-arm service robot and stable operation method |
| CN110531866B (en) * | 2019-10-29 | 2020-03-13 | 深圳市瑞立视多媒体科技有限公司 | Method for performing attitude calculation based on improved inverse kinematics and related equipment |
-
2020
- 2020-05-07 CN CN202010379166.4A patent/CN111571586B/en active Active
Also Published As
| Publication number | Publication date |
|---|---|
| CN111571586A (en) | 2020-08-25 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| AU2008235354B2 (en) | Analysis of parallel manipulators | |
| WO2010042619A4 (en) | Forward kinematic solution for a hexapod manipulator and method of use | |
| CN111571586B (en) | Dynamic constraint method for inverse dynamics freedom degree | |
| CN107160401B (en) | Method for solving problem of joint angle deviation of redundant manipulator | |
| CA2083020A1 (en) | System for determining manipulator coordinates | |
| Zhang et al. | Hybrid Lagrange interpolation differential evolution algorithm for path synthesis | |
| Padovan et al. | Self-adaptive predictor-corrector algorithms for static nonlinear structural analysis | |
| CN111958602A (en) | Real-time inverse solution method for wrist offset type 6-axis robot | |
| US7082223B1 (en) | Reducing aliasing artifacts when shaping a digital image | |
| Sui et al. | Genetic algorithm for solving the inverse kinematics problem for general 6r robots | |
| Nenchev et al. | Singularity-consistent kinematic redundancy resolution for the SRS manipulator | |
| CN110394806B (en) | Rehabilitation manipulator based on high-order adaptive learning control | |
| CN1296815C (en) | Marker digit optimizing method in binary system translation | |
| CN116560218A (en) | Fractional order-linear term iterative learning control method and medium | |
| CN114326769B (en) | Robot motion correction method and device, robot control equipment and storage medium | |
| CN109822571B (en) | Control method, device and equipment for mechanical arm of assembling machine | |
| CN111482968A (en) | Six-degree-of-freedom offset robot inverse solution method based on BFS algorithm | |
| CN115805587A (en) | Motion analysis method and device of seven-axis robot and electronic equipment | |
| CN115525863A (en) | Kinematics solving method and system for seven-degree-of-freedom teleoperation mechanical arm system | |
| CN113434982B (en) | Inverse kinematics solution method of electric intelligent bionic climbing robot | |
| Takayama et al. | A new approach to synthesizing free motions of robotic manipulators based on a concept of unit motions | |
| CN111583366A (en) | Rapid reverse dynamics angle hard constraint method | |
| CN117733872B (en) | Series robot inverse kinematics control method based on directional performance | |
| Kaveh et al. | Plastic analysis of planar frames using kinematic method and genetic algorithm | |
| CN113910220B (en) | Robot operation pose optimization method |
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 |