[go: up one dir, main page]
More Web Proxy on the site http://driver.im/

CN112256047A - 一种基于四元数的四足姿态控制策略 - Google Patents

一种基于四元数的四足姿态控制策略 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
English (en)
Other versions
CN112256047B (zh
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/zh
Publication of CN112256047A publication Critical patent/CN112256047A/zh
Application granted granted Critical
Publication of CN112256047B publication Critical patent/CN112256047B/zh
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

本发明涉及一种姿态控制策略，具体为一种基于四元数的四足姿态控制策略，此方法计算姿态角较简单，通式利用其计算姿态角可以有效避免出现奇异点，包括如下步骤：步骤一：从陀螺仪中读取每个转轴的实时角速度，通过角速度计算旋转角度，具体的计算方法如下：
Figure DDA0002711426870000011
公式(1)可由四元数推导得到；假设世界坐标系下的一个单位向量为Q，若四元数R(x,y,z)以Q为旋转轴，则旋转后的向量可以表示为：R'＝Q‑1*R*Q,则可得：

Description

一种基于四元数的四足姿态控制策略
技术领域
本发明涉及一种姿态控制策略，具体为一种基于四元数的四足姿态控制策略，此方法计算姿态角较简单，通式利用其计算姿态角可以有效避免出现奇异点。
背景技术
四足仿生机器人作为机器人必须具有很高的运动自主性，在复杂的非结构化环境下，只需少量的人工干预，独立自主实施各种运动。并能根据地形环境的变化，自主做出适当的调整，直观上具有了类似于四足动物或人一样的反应和应变能力。由于在运动过程中，具体的动作指令几乎不可能靠人工实现。需要完全借助开发好的运动控制系统自主生成，所以这套系统必须具有很强的鲁棒性和应变性，才能满足不同地形条件下的需求。而在非结构化环境下，姿态自调整对于四足的稳定性则显的十分关键，所以非常有必要研发出基于四元数的姿态控制方法。
发明内容
本发明要解决的技术问题克服现有的缺陷，提供一种基于四元数的四足姿态控制策略。
为了解决上述技术问题，本发明提供了如下的技术方案：一种基于四元数的四足姿态控制策略，包括如下步骤：步骤一：从陀螺仪中读取每个转轴的实时角速度，通过角速度计算旋转角度，具体的计算方法如下：
Figure BDA0002711426860000021
公式(1)可由四元数推导得到；
假设世界坐标系下的一个单位向量为Q，若四元数R(x,y,z)以Q为旋转轴，则旋转后的向量可以表示为：R'＝Q-1*R*Q,则可得：
Figure BDA0002711426860000022
令
Figure BDA0002711426860000023
则可得：
Figure BDA0002711426860000024
Figure BDA0002711426860000031
Figure BDA0002711426860000032
Figure BDA0002711426860000033
对公式(4)基于时间进行微分则可得到公式(1)；
步骤二：将旋转角度带入旋转矩阵，计算可得四足当前姿态下四个髋关节在世界坐标系中的具体位置；
设四个髋关节在世界坐标系中的向量为P1,P2，P3,P4，四个向量经过旋转矩阵的作用下可得到基于世界坐标系的新向量P1′,P2′，P3′,P4′；即：
P1′＝Rxyz*P1
P2′＝Rxyz*P2
P3′＝Rxyz*P3
P4′＝Rxyz*P4
将旋转前后对应向量进行相减，即ΔP＝P′-P，则可得到髋关节在世界坐标系下的变换量；
步骤三：通过四足的运动学模型获得四个足端的运动轨迹以补偿四足姿态的变化。
作为优选，在步骤二中已得到了髋关节经旋转后在世界坐标系下的变化量，在四足运动学模型中，将髋关节视为基坐标，其余关节的位姿均是通过基坐标来表示。
本发明有益效果：本发明的基于四元数的四足姿态控制策略为仿生四足机器人提供能够根据陀螺仪反馈得到的旋转角速度实时计算对应转轴的旋转角度，最终得到当前姿态下的四足的足端轨迹，进而提高四足稳定性和非结构化环境的通过性
具体实施方式
以下对本发明的优选实施例进行说明，应当理解，此处所描述的优选实施例仅用于说明和解释本发明，并不用于限定本发明。
在具体实施时，从陀螺仪中读取每个转轴的实时角速度，通过角速度计算旋转角度，具体的计算方法如下：
Figure BDA0002711426860000041
公式(1)可由四元数推导得到。我们可以假设世界坐标系下的一个单位向量为Q，其用四元数的表示形式为没有实部的单位四元数，若四元数R(x,y,z)以Q为旋转轴，则旋转后的向量可以表示为：R'＝Q-1*R*Q,则可得：
Figure BDA0002711426860000042
令
Figure BDA0002711426860000043
则可得：
Figure BDA0002711426860000051
Figure BDA0002711426860000052
Figure BDA0002711426860000053
Figure BDA0002711426860000054
Figure BDA0002711426860000055
Figure BDA0002711426860000056
Figure BDA0002711426860000057
Figure BDA0002711426860000058
对(4)基于时间进行微分则可得到公式(1)。
步骤二：将旋转角度带入旋转矩阵，计算可得四足当前姿态下四个髋关节在世界坐标系中的具体位置；
设四个髋关节在世界坐标系中的向量为P1,P2，P3,P4，四个向量经过旋转矩阵的作用下可得到新的向量P1′,P2′，P3′,P4′(基于世界坐标系)。
即：
P1′＝Rxyz*P1
P2′＝Rxyz*P2
P3′＝Rxyz*P3
P4′＝Rxyz*P4
将旋转前后对应向量进行相减，即ΔP＝P′-P，则可得到髋关节在世界坐标系下的变换量。
步骤三：通过四足的运动学模型获得四个足端的运动轨迹以补偿四足姿态的变化。
在步骤二中已经得到了髋关节经旋转后在世界坐标系下的变化量，在四足运动学模型中，将髋关节视为基坐标，其余关节的位姿均是通过基坐标来表示。因此，髋关节在世界坐标系中的变化量则可默认为四足足端的变化量，这个变化量即为足端期望轨迹的补偿量，以上三步骤则完成了四足姿态调整。
以上为本发明较佳的实施方式，本发明所属领域的技术人员还能够对上述实施方式进行变更和修改，因此，本发明并不局限于上述的具体实施方式，凡是本领域技术人员在本发明的基础上所作的任何显而易见的改进、替换或变型均属于本发明的保护范围。

Claims (2)

1.一种基于四元数的四足姿态控制策略，其特征在于：包括如下步骤：步骤一：从陀螺仪中读取每个转轴的实时角速度，通过角速度计算旋转角度，具体的计算方法如下：
Figure FDA0002711426850000011
公式(1)可由四元数推导得到；
假设世界坐标系下的一个单位向量为Q，若四元数R(x,y,z)以Q为旋转轴，则旋转后的向量可以表示为：R'＝Q-1*R*Q,则可得：
Figure FDA0002711426850000012
令
Figure FDA0002711426850000013
则可得：
Figure FDA0002711426850000014
Figure FDA0002711426850000015
Figure FDA0002711426850000016
Figure FDA0002711426850000017
Figure FDA0002711426850000018
Figure FDA0002711426850000021
Figure FDA0002711426850000022
Figure FDA0002711426850000023
对公式(4)基于时间进行微分则可得到公式(1)；
步骤二：将旋转角度带入旋转矩阵，计算可得四足当前姿态下四个髋关节在世界坐标系中的具体位置；
设四个髋关节在世界坐标系中的向量为P1,P2，P3,P4，四个向量经过旋转矩阵的作用下可得到基于世界坐标系的新向量P1′,P2′，P3′,P4′；即：
P1′＝Rxyz*P1
P2′＝Rxyz*P2
P3′＝Rxyz*P3
P4′＝Rxyz*P4
将旋转前后对应向量进行相减，即ΔP＝P′-P，则可得到髋关节在世界坐标系下的变换量；
步骤三：通过四足的运动学模型获得四个足端的运动轨迹以补偿四足姿态的变化。
2.根据权利要求1所述的基于四元数的四足姿态控制策略，其特征在于：在步骤二中已得到了髋关节经旋转后在世界坐标系下的变化量，在四足运动学模型中，将髋关节视为基坐标，其余关节的位姿均是通过基坐标来表示。
CN202011058201.9A 2020-09-30 2020-09-30 一种基于四元数的四足姿态控制策略 Active CN112256047B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202011058201.9A CN112256047B (zh) 2020-09-30 2020-09-30 一种基于四元数的四足姿态控制策略

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011058201.9A CN112256047B (zh) 2020-09-30 2020-09-30 一种基于四元数的四足姿态控制策略

Publications (2)

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

Family

ID=74233466

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011058201.9A Active CN112256047B (zh) 2020-09-30 2020-09-30 一种基于四元数的四足姿态控制策略

Country Status (1)

Country Link
CN (1) CN112256047B (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114252073A (zh) * 2021-11-25 2022-03-29 江苏集萃智能制造技术研究所有限公司 一种机器人姿态数据融合方法

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106525049A (zh) * 2016-11-08 2017-03-22 山东大学 一种基于计算机视觉的四足机器人本体位姿跟踪方法
CN106660206A (zh) * 2014-08-25 2017-05-10 谷歌公司 自然的俯仰和滚动
CN109093626A (zh) * 2018-09-28 2018-12-28 中科新松有限公司 四足机器人的机身姿态控制方法及装置
CN111309039A (zh) * 2020-02-27 2020-06-19 杭州云深处科技有限公司 一种四足机器人姿态控制方法和装置
CN111527461A (zh) * 2018-01-09 2020-08-11 索尼公司 信息处理装置、信息处理方法和程序

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106660206A (zh) * 2014-08-25 2017-05-10 谷歌公司 自然的俯仰和滚动
CN106525049A (zh) * 2016-11-08 2017-03-22 山东大学 一种基于计算机视觉的四足机器人本体位姿跟踪方法
CN111527461A (zh) * 2018-01-09 2020-08-11 索尼公司 信息处理装置、信息处理方法和程序
CN109093626A (zh) * 2018-09-28 2018-12-28 中科新松有限公司 四足机器人的机身姿态控制方法及装置
CN111309039A (zh) * 2020-02-27 2020-06-19 杭州云深处科技有限公司 一种四足机器人姿态控制方法和装置

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 (zh) * 2021-11-25 2022-03-29 江苏集萃智能制造技术研究所有限公司 一种机器人姿态数据融合方法
CN114252073B (zh) * 2021-11-25 2023-09-15 江苏集萃智能制造技术研究所有限公司 一种机器人姿态数据融合方法

Also Published As

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

Similar Documents

Publication Publication Date Title
Koenemann et al. Real-time imitation of human whole-body motions by humanoids
JP3443077B2 (ja) ロボットの運動パターン生成装置及び運動パターン生成方法、並びにロボット
CN109093626B (zh) 四足机器人的机身姿态控制方法及装置
CN110202580B (zh) 一种扰动恢复的仿人机器人空间柔顺控制模型构建方法
CN113835429B (zh) 一种仿生双足机器人的控制方法及仿生双足机器人
CN106607910B (zh) 一种机器人实时模仿方法
CN104986241B (zh) 四足机器人的步态规划方法
KR100824179B1 (ko) 보행식 이동 로봇의 동작 제어 장치 및 동작 제어 방법,및 보행식 이동 로봇을 위한 동작 유닛 생성 방법
CN108572553B (zh) 一种四足机器人的运动闭环控制方法
CN112051741A (zh) 一种双足机器人动态运动生成与控制方法
CN109760761B (zh) 一种基于生物仿生原理和直觉的四足机器人运动控制方法
CN111625002B (zh) 一种仿人机器人爬楼步态规划与控制的方法
CN110039544A (zh) 基于三次样条插值的仿人足球机器人步态规划
Babič et al. Human sensorimotor learning for humanoid robot skill synthesis
Elfes et al. The multilegged autonomous explorer (MAX)
WO2004033160A1 (ja) ロボット装置の動作制御装置及び動作制御方法
CN112256047B (zh) 一种基于四元数的四足姿态控制策略
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
JP3674779B2 (ja) 脚式移動ロボットのための動作制御装置及び動作制御方法、並びにロボット装置
Kondo et al. Real-time posture imitation of biped humanoid robot based on particle filter with simple joint control for standing stabilization
JP2003117858A (ja) ロボットの歩行制御装置及び歩行制御方法
CN109693235A (zh) 一种仿人眼视觉跟踪装置及其控制方法
Pan et al. Design and kinematic analysis of a rope-driven linkage frog-like swimming robot with multidirectional controllable motion
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