CN112256047A - Quaternion-based four-foot attitude control strategy - Google Patents

Quaternion-based four-foot attitude control strategy Download PDF

Info

Publication number
CN112256047A
CN112256047A CN202011058201.9A CN202011058201A CN112256047A CN 112256047 A CN112256047 A CN 112256047A CN 202011058201 A CN202011058201 A CN 202011058201A CN 112256047 A CN112256047 A CN 112256047A
Authority
CN
China
Prior art keywords
coordinate system
world coordinate
quaternion
xyz
steps
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN202011058201.9A
Other languages
Chinese (zh)
Other versions
CN112256047B (en
Inventor
张佳丽
陈云川
刘志鹏
骆敏舟
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Institute of Intelligent Manufacturing Technology JITRI
Original Assignee
Institute of Intelligent Manufacturing Technology JITRI
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Institute of Intelligent Manufacturing Technology JITRI filed Critical Institute of Intelligent Manufacturing Technology JITRI
Priority to CN202011058201.9A priority Critical patent/CN112256047B/en
Publication of CN112256047A publication Critical patent/CN112256047A/en
Application granted granted Critical
Publication of CN112256047B publication Critical patent/CN112256047B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/08—Control of attitude, i.e. control of roll, pitch, or yaw
    • G05D1/0891—Control of attitude, i.e. control of roll, pitch, or yaw specially adapted for land vehicles

Landscapes

  • Engineering & Computer Science (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Manipulator (AREA)

Abstract

The invention relates to an attitude control strategy, in particular to a quadruped attitude control strategy based on quaternion, the method has simpler calculation of attitude angles, and a general formula can effectively avoid singular points by utilizing the calculation of the attitude angles, and comprises the following steps: the method comprises the following steps: reading the real-time angular velocity of each rotating shaft from the gyroscope, and calculating the rotating angle through the angular velocity, wherein the specific calculation method comprises the following steps:
Figure DDA0002711426870000011
formula (1) can be derived from quaternions; assuming that a unit vector in the world coordinate system is Q, if the quaternion R (x, y, z) takes Q as a rotation axis, the rotated vector can be expressed as: r ═ Q‑1R Q, then:

Description

Quaternion-based four-foot attitude control strategy
Technical Field
The invention relates to an attitude control strategy, in particular to a quadruped attitude control strategy based on quaternion.
Background
The four-footed bionic robot has to have high movement autonomy as a robot, and only needs a small amount of manual intervention to independently and autonomously implement various movements in a complex unstructured environment. And proper adjustment can be automatically made according to the change of the terrain environment, and the device has the reaction and strain capabilities similar to those of a quadruped animal or a human in a visual manner. Since the specific motion instruction is almost impossible to be implemented manually during the movement. The system needs to be generated by a developed motion control system, so that the system has strong robustness and variability to meet the requirements under different terrain conditions. In an unstructured environment, the self-adjustment of the posture is very critical to the stability of the four feet, so that a posture control method based on quaternion is very necessary to be developed.
Disclosure of Invention
The technical problem to be solved by the invention is to overcome the existing defects and provide a four-footed attitude control strategy based on quaternion.
In order to solve the technical problems, the invention provides the following technical scheme: a quaternion-based four-footed attitude control strategy comprises the following steps: the method comprises the following steps: reading the real-time angular velocity of each rotating shaft from the gyroscope, and calculating the rotating angle through the angular velocity, wherein the specific calculation method comprises the following steps:
Figure BDA0002711426860000021
formula (1) can be derived from quaternions;
assuming that a unit vector in the world coordinate system is Q, if the quaternion R (x, y, z) takes Q as a rotation axis, the rotated vector can be expressed as: r ═ Q-1R Q, then:
Figure BDA0002711426860000022
order to
Figure BDA0002711426860000023
Then it can be obtained:
Figure BDA0002711426860000024
Figure BDA0002711426860000031
Figure BDA0002711426860000032
Figure BDA0002711426860000033
differentiating the formula (4) based on time to obtain a formula (1);
step two: the rotation angle is substituted into a rotation matrix, and the specific positions of the four hip joints in the world coordinate system under the current postures of the four feet are obtained through calculation;
let the vectors of four hip joints in the world coordinate system be P1,P2，P3,P4The four vectors can obtain a new vector P based on the world coordinate system under the action of the rotation matrix1′,P2′，P3′,P4'; namely:
P1′＝Rxyz*P1
P2′＝Rxyz*P2
P3′＝Rxyz*P3
P4′＝Rxyz*P4
subtracting the corresponding vectors before and after rotation, namely, if delta P is P' -P, obtaining the transformation quantity of the hip joint in a world coordinate system;
step three: and obtaining the motion tracks of the four foot ends through a kinematic model of the four feet so as to compensate the change of the posture of the four feet.
Preferably, the amount of change of the hip joint in the world coordinate system after rotation is obtained in the second step, and in the four-footed kinematic model, the hip joint is regarded as base coordinates, and the poses of the rest joints are all represented by the base coordinates.
The invention has the beneficial effects that: the quaternion-based four-foot attitude control strategy provided by the invention can be used for providing the bionic four-foot robot with the rotating angles of the corresponding rotating shafts which can be calculated in real time according to the rotating angular velocity obtained by the feedback of the gyroscope, and finally, the foot end tracks of the four feet in the current attitude are obtained, so that the stability of the four feet and the trafficability of an unstructured environment are improved
Detailed Description
The following description of the preferred embodiments of the present invention is provided for the purpose of illustration and description, and is in no way intended to limit the invention.
In specific implementation, the real-time angular velocity of each rotating shaft is read from the gyroscope, and the rotating angle is calculated through the angular velocity, wherein the specific calculation method comprises the following steps:
Figure BDA0002711426860000041
equation (1) can be derived from quaternions. We can assume that a unit vector in the world coordinate system is Q, which is expressed in terms of quaternion as a unit quaternion without real part, and if quaternion R (x, y, z) is taken as a rotation axis with Q, the rotated vector can be expressed as: r ═ Q-1R Q, then:
Figure BDA0002711426860000042
order to
Figure BDA0002711426860000043
Then it can be obtained:
Figure BDA0002711426860000051
Figure BDA0002711426860000052
Figure BDA0002711426860000053
Figure BDA0002711426860000054
Figure BDA0002711426860000055
Figure BDA0002711426860000056
Figure BDA0002711426860000057
Figure BDA0002711426860000058
differentiating (4) based on time yields equation (1).
Step two: the rotation angle is substituted into a rotation matrix, and the specific positions of the four hip joints in the world coordinate system under the current postures of the four feet are obtained through calculation;
let the vectors of four hip joints in the world coordinate system be P1,P2，P3,P4The new vector P can be obtained by the four vectors under the action of the rotation matrix1′,P2′，P3′,P4' (based on the world coordinate system).
Namely:
P1′＝Rxyz*P1
P2′＝Rxyz*P2
P3′＝Rxyz*P3
P4′＝Rxyz*P4
and subtracting the corresponding vectors before and after rotation, namely, if delta P is equal to P' -P, the transformation amount of the hip joint in the world coordinate system can be obtained.
Step three: and obtaining the motion tracks of the four foot ends through a kinematic model of the four feet so as to compensate the change of the posture of the four feet.
And step two, obtaining the variation of the hip joint in a world coordinate system after the hip joint is rotated, and in the four-foot kinematics model, regarding the hip joint as a base coordinate, and expressing the poses of the other joints through the base coordinate. Therefore, the variation of the hip joint in the world coordinate system can be defaulted to the variation of the four feet, and the variation is the compensation of the expected track of the feet, and the posture adjustment of the four feet is completed through the three steps.
The above embodiments are preferred embodiments of the present invention, and those skilled in the art can make variations and modifications to the above embodiments, therefore, the present invention is not limited to the above embodiments, and any obvious improvements, substitutions or modifications made by those skilled in the art based on the present invention are within the protection scope of the present invention.

Claims (2)

1. A four-footed attitude control strategy based on quaternion is characterized in that: the method comprises the following steps: the method comprises the following steps: reading the real-time angular velocity of each rotating shaft from the gyroscope, and calculating the rotating angle through the angular velocity, wherein the specific calculation method comprises the following steps:
Figure FDA0002711426850000011
formula (1) can be derived from quaternions;
assuming that a unit vector in the world coordinate system is Q, if the quaternion R (x, y, z) takes Q as the rotation axis, the rotation is performedThe transformed vector can be expressed as: r ═ Q-1R Q, then:
Figure FDA0002711426850000012
order to
Figure FDA0002711426850000013
Then it can be obtained:
Figure FDA0002711426850000014
Figure FDA0002711426850000015
Figure FDA0002711426850000016
Figure FDA0002711426850000017
Figure FDA0002711426850000018
Figure FDA0002711426850000021
Figure FDA0002711426850000022
Figure FDA0002711426850000023
differentiating the formula (4) based on time to obtain a formula (1);
step two: the rotation angle is substituted into a rotation matrix, and the specific positions of the four hip joints in the world coordinate system under the current postures of the four feet are obtained through calculation;
let the vectors of four hip joints in the world coordinate system be P1,P2，P3,P4The four vectors can obtain a new vector P based on the world coordinate system under the action of the rotation matrix1′,P2′，P3′,P4'; namely:
P1′＝Rxyz*P1
P2′＝Rxyz*P2
P3′＝Rxyz*P3
P4′＝Rxyz*P4
subtracting the corresponding vectors before and after rotation, namely, if delta P is P' -P, obtaining the transformation quantity of the hip joint in a world coordinate system;
step three: and obtaining the motion tracks of the four foot ends through a kinematic model of the four feet so as to compensate the change of the posture of the four feet.
2. The quaternion-based four-footed attitude control strategy of claim 1 wherein: and step two, obtaining the variation of the hip joint in a world coordinate system after the hip joint is rotated, and in the four-foot kinematics model, regarding the hip joint as a base coordinate, wherein the poses of the other joints are all represented by the base coordinate.
CN202011058201.9A 2020-09-30 2020-09-30 Quaternion-based four-footed attitude control strategy Active CN112256047B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202011058201.9A CN112256047B (en) 2020-09-30 2020-09-30 Quaternion-based four-footed attitude control strategy

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011058201.9A CN112256047B (en) 2020-09-30 2020-09-30 Quaternion-based four-footed attitude control strategy

Publications (2)

Publication Number Publication Date
CN112256047A true CN112256047A (en) 2021-01-22
CN112256047B CN112256047B (en) 2023-04-11

Family

ID=74233466

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011058201.9A Active CN112256047B (en) 2020-09-30 2020-09-30 Quaternion-based four-footed attitude control strategy

Country Status (1)

Country Link
CN (1) CN112256047B (en)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114252073A (en) * 2021-11-25 2022-03-29 江苏集萃智能制造技术研究所有限公司 Robot attitude data fusion method

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106525049A (en) * 2016-11-08 2017-03-22 山东大学 Quadruped robot body posture tracking method based on computer vision
CN106660206A (en) * 2014-08-25 2017-05-10 谷歌公司 Natural pitch and roll
CN109093626A (en) * 2018-09-28 2018-12-28 中科新松有限公司 The fuselage attitude control method and device of quadruped robot
CN111309039A (en) * 2020-02-27 2020-06-19 杭州云深处科技有限公司 Four-footed robot attitude control method and device
CN111527461A (en) * 2018-01-09 2020-08-11 索尼公司 Information processing apparatus, information processing method, and program

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106660206A (en) * 2014-08-25 2017-05-10 谷歌公司 Natural pitch and roll
CN106525049A (en) * 2016-11-08 2017-03-22 山东大学 Quadruped robot body posture tracking method based on computer vision
CN111527461A (en) * 2018-01-09 2020-08-11 索尼公司 Information processing apparatus, information processing method, and program
CN109093626A (en) * 2018-09-28 2018-12-28 中科新松有限公司 The fuselage attitude control method and device of quadruped robot
CN111309039A (en) * 2020-02-27 2020-06-19 杭州云深处科技有限公司 Four-footed robot attitude control method and device

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
何丽媛 等: "基于马达代数的机械臂运动学建模", 《机械设计与制造工程》 *

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114252073A (en) * 2021-11-25 2022-03-29 江苏集萃智能制造技术研究所有限公司 Robot attitude data fusion method
CN114252073B (en) * 2021-11-25 2023-09-15 江苏集萃智能制造技术研究所有限公司 Robot attitude data fusion method

Also Published As

Publication number Publication date
CN112256047B (en) 2023-04-11

Similar Documents

Publication Publication Date Title
Koenemann et al. Real-time imitation of human whole-body motions by humanoids
CN109093626B (en) Body attitude control method and device of quadruped robot
JP3443077B2 (en) Robot motion pattern generation device and motion pattern generation method, and robot
KR100824179B1 (en) Device and method for controlling motion of legged mobile robot, and motion unit generating method for legged mobile robot
CN108572553B (en) Motion closed-loop control method of quadruped robot
US20130079929A1 (en) Robot and control method thereof
CN109202901A (en) A kind of biped robot's stair climbing gait planning method, apparatus and robot
CN109760761B (en) Four-footed robot motion control method based on bionics principle and intuition
CN113835429B (en) Control method of bionic bipedal robot and bionic bipedal robot
JP5052013B2 (en) Robot apparatus and control method thereof
CN112051741A (en) Dynamic motion generation and control method for biped robot
CN111625002B (en) Stair-climbing gait planning and control method of humanoid robot
CN110039544A (en) Apery Soccer robot gait planning based on cubic spline interpolation
Babič et al. Human sensorimotor learning for humanoid robot skill synthesis
Elfes et al. The multilegged autonomous explorer (MAX)
CN112256047B (en) Quaternion-based four-footed attitude control strategy
WO2004033160A1 (en) Robot device operation control device and operation control method
Song et al. Development of a bipedal robot that walks like an animation character
Montecillo-Puente et al. On real-time whole-body human to humanoid motion transfer
Ding et al. Online center of mass detection for quadruped robots in trot gait motion
Kondo et al. Real-time posture imitation of biped humanoid robot based on particle filter with simple joint control for standing stabilization
CN109693235A (en) A kind of Prosthetic Hand vision tracking device and its control method
JP3674779B2 (en) Motion control device and motion control method for legged mobile robot, and robot device
JP2003117858A (en) Method and device for control of robot walk
Lim et al. Control design to achieve dynamic walking on a bipedal robot with compliance

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