一种基于四元数的四足姿态控制策略制造技术

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

【技术实现步骤摘要】
一种基于四元数的四足姿态控制策略


[0001]本专利技术涉及一种姿态控制策略，具体为一种基于四元数的四足姿态控制策略，此方法计算姿态角较简单，通式利用其计算姿态角可以有效避免出现奇异点。

技术介绍

[0002]四足仿生机器人作为机器人必须具有很高的运动自主性，在复杂的非结构化环境下，只需少量的人工干预，独立自主实施各种运动。并能根据地形环境的变化，自主做出适当的调整，直观上具有了类似于四足动物或人一样的反应和应变能力。由于在运动过程中，具体的动作指令几乎不可能靠人工实现。需要完全借助开发好的运动控制系统自主生成，所以这套系统必须具有很强的鲁棒性和应变性，才能满足不同地形条件下的需求。而在非结构化环境下，姿态自调整对于四足的稳定性则显的十分关键，所以非常有必要研发出基于四元数的姿态控制方法。

技术实现思路

[0003]本专利技术要解决的技术问题克服现有的缺陷，提供一种基于四元数的四足姿态控制策略。
[0004]为了解决上述技术问题，本专利技术提供了如下的技术方案：一种基于四元数的四足姿态控制策略，包括如下步骤：步骤一：从陀螺仪中读取每个转轴的实时角速度，通过角速度计算旋转角度，具体的计算方法如下：
[0005][0006]公式(1)可由四元数推导得到；
[0007]假设世界坐标系下的一个单位向量为Q，若四元数R(x,y,z)以Q为旋转轴，则旋转后的向量可以表示为：R'＝Q-1
*R*Q,则可得：
[0008][0009]令
[0010][0011]则可得：
[0012][0013][0014][0015][0016]对公式(4)基于时间进行微分则可得到公式(1)；
[0017]步骤二：将旋转角度带入旋转矩阵，计算可得四足当前姿态下四个髋关节在世界坐标系中的具体位置；
[0018]设四个髋关节在世界坐标系中的向量为P1,P2，P3,P4，四个向量经过旋转矩阵的作用下可得到基于世界坐标系的新向量P1′
,P2′
，P3′
,P4′
；即：
[0019]P1′
＝R
xyz
*P1[0020]P2′
＝R
xyz
*P2[0021]P3′
＝R
xyz
*P3[0022]P4′
＝R
xyz
*P4[0023]将旋转前后对应向量进行相减，即ΔP＝P
′-
P，则可得到髋关节在世界坐标系下的变换量；
[0024]步骤三：通过四足的运动学模型获得四个足端的运动轨迹以补偿四足姿态的变化。
[0025]作为优选，在步骤二中已得到了髋关节经旋转后在世界坐标系下的变化量，在四足运动学模型中，将髋关节视为基坐标，其余关节的位姿均是通过基坐标来表示。
[0026]本专利技术有益效果：本专利技术的基于四元数的四足姿态控制策略为仿生四足机器人提供能够根据陀螺仪反馈得到的旋转角速度实时计算对应转轴的旋转角度，最终得到当前姿态下的四足的足端轨迹，进而提高四足稳定性和非结构化环境的通过性
具体实施方式
[0027]以下对本专利技术的优选实施例进行说明，应当理解，此处所描述的优选实施例仅用于说明和解释本专利技术，并不用于限定本专利技术。
[0028]在具体实施时，从陀螺仪中读取每个转轴的实时角速度，通过角速度计算旋转角度，具体的计算方法如下：
[0050]P4′
＝R
xyz
*P4[0051]将旋转前后对应向量进行相减，即ΔP＝P
′-
P，则可得到髋关节在世界坐标系下的变换量。
[0052]步骤三：通过四足的运动学模型获得四个足端的运动轨迹以补偿四足姿态的变化。
[0053]在步骤二中已经得到了髋关节经旋转后在世界坐标系下的变化量，在四足运动学模型中，将髋关节视为基坐标，其余关节的位姿均是通过基坐标来表示。因此，髋关节在世界坐标系中的变化量则可默认为四足足端的变化量，这个变化量即为足端期望轨迹的补偿量，以上三步骤则完成了四足姿态调整。
[0054]以上为本专利技术较佳的实施方式，本专利技术所属领域的技术人员还能够对上述实施方式进行变更和修改，因此，本专利技术并不局限于上述的具体实施方式，凡是本领域技术人员在本专利技术的基础上所作的任何显而易见的改进、替换或变型均属于本专利技术的保护范围。
本文档来自技高网...

【技术保护点】

【技术特征摘要】
P2′
＝R
xyz
*P2P3′
＝R
xyz
*P3P4′
＝R
xyz
*P4将旋转前后对应向量进行相减，即ΔP＝P
′-
P，则可得到髋关节在世界坐标系下...

【专利技术属性】
技术研发人员：张佳丽，陈云川，刘志鹏，骆敏舟，
申请(专利权)人：江苏集萃智能制造技术研究所有限公司，
类型：发明
国别省市：