CN113359462B - 一种基于扰动解耦与补偿的仿生眼稳像系统及方法 - Google Patents

Abstract

本发明提供了一种基于扰动解耦与补偿的仿生眼稳像系统及方法，具体为：稳定化线性控制器根据IMU反馈的姿态角度φ_i、期望的姿态角度φ_d和其微分量

得到控制输入向量y；结合IMU的各项反馈的姿态角度φ_i、关节编码器反馈关节角度θ和角速度

进行正逆运动学和速度运动学的计算，完成非线性补偿与解耦，最终得到稳像方法的控制律u；控制律u将作为颈部关节电机运动的参考值，通过驱动器控制关节电机运动，完成基于扰动解耦与补偿的稳像方法控制。本发明能够实现大扰动下的仿生眼快速稳像控制，控制系统简单的、对硬件要求低。

Description

一种基于扰动解耦与补偿的仿生眼稳像系统及方法

技术领域

本发明属于仿生眼技术领域，具体涉及一种基于扰动解耦与补偿的仿生眼稳像系统及方法。

背景技术

仿生眼已经可以让机器人获得视觉信息，但是运动中不稳定的图像序列很难提取有效信息，因而，机器人如何在运动过程中获取稳定的视觉信息成为研究者们关注的焦点。研究者们力图在机器人上实现头部稳定，不仅可以保持机器人运动中的姿态，还可以为获取稳定视觉信息提供基础。

人类的头部稳定性已经被神经科学家进行了广泛的研究，而机器人领域的研究人员们逐渐将类似人类的控制用来改善机器人的惯性感知、姿态控制。在这里，人类的前庭系统引起了广泛的关注，它在日常活动中扮演重要角色，参与各种各样的功能。研究表明，只有前庭信息才能重建当前和过去被动身体的位置。使用惯性测量元件模拟人类的前庭系统，在机器人移动过程中提供一个稳定的参考框架，该框架与其他组成合作，最终实现机器人头部稳定，这已经成为研究者们的共识。

目前现有的技术方案国外的有东京工业大学的蛇形机器人，通过控制颈部的运动消除身体的起伏影响来稳定头部，但在控制策略中并没有外部传感器的参与；纽约市立大学的一种机器学习算法的实现，利用嵌入在机器人头部的人工前庭系统的信号，可以快速学习以补偿不存在稳定机制时出现的头部运动；葡萄牙国立米尼奥大学的一种使用中央模式发生器(CPG)和遗传算法在四足机器人上产生头部运动稳定的组合方法；西班牙埃斯特雷马杜拉大学的一种三自由度模拟器的平衡和方向控制；意大利生物机器人研究所的Paolo Dario团队设计的一个使用反馈的实际头部绝对RPY角度反馈的控制器，开发的逆运动学控制器(IK Controller)、逆雅可比控制器(IJ Controller)和基于反馈误差学习的生物启发式的自适应控制器(FEL Controller)，并在VCR-OKR-VCR稳定系统上提出了综合的控制模型上，实现了于前庭丘反射(VCR)、前庭眼反射(VOR)和视动反射(OKR)协调的注视稳定模型等等。

国内的技术方案有上海交通大学的一种头眼协调运动的控制策略，研究了机器人头部转动时双目的补偿运动模型及相应的控制算法；东南大学的一种蛇形机器人头部的稳定控制方法；北京理工大学的机械稳像算法通过重力补偿和扰动补偿，考虑机器人头部设计的偏心问题，使得控制系统线性化等等。

随着仿生学和机器学习等研究的进步，一些研究人员则是集中于生物机理的研究或者各种稳定策略的整合，此外则是在机器学习的方向上进行探索。机器学习的方法虽然也取得了一些比较好的结果，但是对于硬件平台的要求比较严格，成本较高，并且控制系统变得复杂，对于一些没有高运算力的控制器来说并不适用。

发明内容

针对现有技术中存在不足，本发明提供了一种基于扰动解耦与补偿的仿生眼稳像系统及方法，控制系统简单的、对硬件要求低，且能够精确辨识外部速度扰动。

本发明是通过以下技术手段实现上述技术目的的。

一种基于扰动解耦与补偿的仿生眼稳像方法，具体为：

稳定化线性控制器根据IMU反馈的姿态角度φ_i、期望的姿态角度φ_d和其微分量

得到控制输入向量y；

结合IMU的各项反馈的姿态角度φ_i、关节编码器反馈关节角度θ和角速度

进行正逆运动学和速度运动学的计算，完成非线性补偿与解耦，最终得到稳像方法的控制律u；

控制律u将作为颈部关节电机运动的参考值，通过驱动器控制关节电机运动，完成基于扰动解耦与补偿的稳像方法控制。

上述技术方案中，所述控制输入向量y为：

其中，K_p为比例调节系数，K_i为积分调节系数，φ_e＝φ_d-φ_i。

上述技术方案中，所述控制律u为：

其中，V^-1＝(^WR₀ ⁰J_O)^-1，^WR_a为世界坐标系到坐标系0的旋转矩阵，雅克比矩阵

为雅克比矩阵；^WR_t ⁱω_i等于IMU坐标系相对世界坐标系的角速度^Wω_i

上述技术方案中，所述IMU坐标系相对世界坐标系的角速度^Wω_i为：

其中，^Wω₀为0坐标系相对世界坐标系的角速度矢量。

上述技术方案中，还包括根据

进行基于扰动解耦与补偿的稳像方法的收敛性证明。

上述技术方案中，所述正逆运动学的计算根据坐标系j和坐标系j-1的变换矩阵、IMU坐标系相对于世界坐标系的旋转矩阵和雅可比矩阵⁰J_O进行的；

所述坐标系j和坐标系j-1的变换矩阵为：

其中，α_j是z_j-1轴和z_j轴之间的夹角；a_j是使得z_j-1经过坐标系j的原点时，沿x_j-1轴平移的距离；d_j是使x_j-1和x_j处于同一平面时，沿z_j-1轴平移的距离；θ_j为x_j-1轴和x_j轴之间的夹角；

所述IMU坐标系相对于世界坐标系的旋转矩阵为：

其中，R_x、R_y和R_z分别表示围绕x、y、z旋转的旋转矩阵，ψ、

和

分别为IMU绕x轴、y轴、z轴旋转的角度。

上述技术方案中，根据所述机器人速度运动学，建立IMU角速度、关节角速度和扰动角速度之间的关系为：

^Wω_i＝^WR₀ ⁰ω_i+^Wω₀。

一种基于扰动解耦与补偿的仿生眼稳像系统，包括：

稳定化线性控制器，用于根据IMU反馈的姿态角度φ_i、期望的姿态角度φ_d和其微分量

得到控制输入向量y；

非线性补偿与解耦器，用于结合IMU的各项反馈的姿态角度φ_i、关节编码器反馈关节角度θ和角速度

进行正逆运动学和速度运动学的计算，得到稳像方法的控制律u。

本发明的有益效果为：

本发明基于扰动解耦与补偿的仿生眼稳像方法，通过IMU反馈和关节编码器的反馈，对外部扰动进行了精确的识别，推断得到等量的外部扰动，将其加入到稳像控制律中，在每一个关节进行补偿，实现大扰动下的仿生眼快速稳像控制；并对稳像方法给出收敛性证明，证明稳像方法的控制是稳定且收敛的。本发明的优点有：

(1)不使用任何机器学习算法，对硬件平台要求不高，甚至可运行于普通单片机上；

(2)能够精确辨识外部速度扰动，从而可以更精确地实现速度补偿；

(3)对比被动式的电子稳像，本方法可以保证目标不丢失。

附图说明

图1为本发明仿生眼DH模型示意图；

图2为本发明涉及的坐标系关系示意图；

图3为本发明未使用稳像控制时仿生眼状态图；

图4为本发明使用稳像控制时仿生眼状态图；

图5为本发明仿生眼稳像的精确线性化控制示意图；

图6(a)为本发明二阶系统在过阻尼情况下的示例根位置图；

图6(b)为本发明二阶系统在临界阻尼情况下的示例根位置图；

图6(c)为本发明二阶系统在欠阻尼情况下的示例根位置图；

图7为本发明二阶系统在三种情况下的响应图；

图8为本发明根位置与瞬态响应属性之间的关系图；

图9为本发明基于扰动解耦与补偿的稳像算法控制框架图。

具体实施方式

下面结合附图以及具体实施例对本发明作进一步的说明，但本发明的保护范围并不限于此。

本发明一种基于扰动解耦与补偿的仿生眼稳像方法，具体如下：

(1)仿生眼DH模型中的坐标系变换关系

根据仿生眼的机械结构，使用标准DH法，建立数学模型(如图1所示)。0表示仿生眼颈部偏航关节，1表示仿生眼颈部滚转关节，2表示仿生眼颈部俯仰关节，3l和3r分别表征左右眼球的俯仰关节，i表示装配于仿生眼颈部末端执行器IMU(惯性测量元件)，4和6表示仿生眼眼部偏航关节，5和7表示末端坐标系。

仿生眼系统的DH参数如表1所示。建立DH模型后，坐标系j关于坐标系j-1的位置和方向完全由θ_j、d_j、a_j和α_j四个参数确定。这四个参数中，a_j和α_j始终为常数，只取决于相邻关节之间的几何连接关系，由于仿生眼的所有关节均为转动型，从而d_j为常量，θ_j为变量。这四个参数的确定方式如下：

①θ_j：x_j-1轴和x_j轴之间的夹角，当绕z_j-1轴逆时针转动时取正，用q_j表示θ_j的初始角度。

②d_j：使得x_j-1和x_j处于同一平面时，沿z_j-1轴平移的距离。

③a_j：使得z_j-1经过坐标系j的原点时，沿x_j-1轴平移的距离。

④α_j：z_j-1轴和z_j轴之间的夹角，当绕x_j轴逆时针转动时取正。

表1 DH参数表

标准DH模型中，坐标系j和坐标系j-1的变换矩阵关系如公式(1)所示。

坐标系j和坐标系j-1的旋转矩阵计算如公式(2)所示。

仿生眼DH模型涉及的坐标系之间关系示意如图2所示，即这里面需要考虑到世界坐标系W、IMU坐标系、仿生眼底部固连的坐标系0、关节坐标系1、坐标系2之间的变换关系(参见图2)。推导相邻坐标系间的变换关系，从而可以得到坐标系0到坐标系2的旋转矩阵计算。

使用cθ表示cosθ，使用sθ表示sinθ，可以得到坐标系0到IMU坐标系的旋转矩阵⁰R_i的表达式为：

七自由度仿生眼使用的IMU按照动轴转动，旋转顺序为z→y→x，其中绕x轴旋转的角度记为ψ、绕y轴旋转的角度记为

绕z轴旋转的角度记为

根据这些姿态角(旋转角度)可以得到IMU坐标系相对于世界坐标系的旋转矩阵，如下：

其中R_x、R_y和R_z分别表示围绕x、y、z旋转的旋转矩阵，计算方式如下：

对于世界坐标系和IMU坐标系之间的变换可以通过IMU输出的姿态角进行计算，结合公式(5)，可以得到世界坐标系到IMU坐标系的旋转矩阵计算：

结合公式(4)和(9)，可以得到世界坐标系到坐标系0的旋转矩阵计算：

^WR₀＝^WR_i ⁱR₀ (10)

公式中ⁱR₀为⁰R_i的逆矩阵，而结合旋转矩阵的特点，⁰R_i的逆矩阵即本身的转置，从而可以得到：

(2)仿生眼DH模型中的角速度关系

①关节角速度间的变换关系

仿生眼颈部关节分别与末端执行器(IMU)线速度、角速度的微分运动学将由雅可比矩阵描述，仿生眼颈部可以视作一个三自由度的机械臂，只考虑旋转速度的变换，可以得到雅可比矩阵：

⁰J_O＝[z₀ z₁ z₂] (12)

这里的z₀＝[0,0,1]^T，z₁和z₂分别由旋转矩阵⁰R₁和⁰R₂的第三列给出，从而有如下的计算方式：

结合公式(12)-(13)，可以得到雅可比矩阵的计算如下：

对雅可比矩阵求逆，可以得到：

根据雅可比矩阵，可以建立关节角速度、IMU相对坐标系0的角速度之间的关系：

θ为仿生眼颈部关节的旋转角度，

为仿生眼颈部关节的旋转角速度。

②IMU角速度的变换关系

惯性测量元件(IMU)中的陀螺仪可以测得角速度ⁱω_i，这是相对于IMU自身旋转轴的角速度。根据连杆间速度关系，结合IMU测量的姿态角

可以得到IMU坐标系相对世界坐标系的角速度：

^Wω_i＝^WR_i ⁱω_i (17)

这里的^WR_i的计算参考公式(9)。

③角速度与欧拉角微分关系

角速度矢量和欧拉角微分

间存在如下关系：

其中

一个雅可比矩阵，表示了角度组合速度矢量和角速度矢量的关系，同时也是这个角度组合瞬时值的函数，公式中的

表达式如下：

结合公式(18)-(19)，可以得到IMU角速度矢量的计算公式(20)以及0坐标系相对世界坐标系的角速度矢量的计算公式(21)：

④角速度关系

结合DH模型和实际角速度计算，可以建立IMU角速度、关节角速度和扰动角速度之间的关系，根据机器人学中速度运动学的知识可以得到：

^Wω_i＝^WR₀ ⁰ω_i+^Wω₀ (22)

因为仿生眼在运动过程中，只有底座与机器人或其他平台固连，因而，仿生眼底座的角速度即受到的扰动角速度，即坐标系0相对于世界坐标系的角速度。将公式(16)带入公式(22)，可以得到：

公式(23)是基于扰动解耦与补偿的仿生眼稳像方法的核心公式，这一公式直接给出了DH模型中涉及的各坐标系旋转速度之间的关系，根据这一公式可以将扰动速度完全解耦到每一个颈部关节上。

(3)基于扰动解耦与补偿的稳像方法

对于稳像而言，需要考虑仿生眼的两种状态：一种是仿生眼受到外部扰动，并未进行稳像控制的状态，如图3所示。此时仿生眼的整体姿态会因为外部扰动的变化而变化，进而导致视觉系统出现抖动。一种是仿生眼受到外部扰动的同时，进行稳像控制的状态，如图4所示。此时在稳像控制器的作用下，虽然仿生眼底座的姿态会随着外部扰动变化而变化，但是通过控制颈部关节的运动，可以保持视觉系统稳定。从而稳像控制的本质问题可以转换为跟踪关节空间轨迹的问题，即通过传感器(装配于颈部末端的IMU和每一关节的编码器)获取实时姿态角度和角速度，将扰动解耦到每一个颈部关节上，此时通过速度运动学和逆运动学构造稳像控制律。

为了简化多个变量的表示，约定使用

表示欧拉角，并将公式(20)-(21)带入到公式(23)中，可以得到如下的表达形式：

对上式进行变换可以得到：

在公式(25)中，注意到关节的旋转角速度

是可以控制的变量，从而使用控制律u来构造仿生眼稳像控制的系统状态函数如下：

在这里，令y代替

可以得到基于扰动解耦与补偿的稳像方法控制律如下：

将公式(25)带入稳像方法的控制律中，可以得到：

为了进一步简化表示控制律，使用如下的简化方式：

V＝^WR₀ ⁰J_O (29)

对其求逆，可以得到：

将公式(29)带入公式(28)中，可以得到：

公式(31)中的y表示新的输入向量，其表达式尚待确定。

如图5所示，控制律采用逆运动学和速度运动学的组合控制形式，因为该控制律是基于仿生眼的速度运动学和逆运动学解算得到的，公式中控制的系统相对于新输入向量y是解耦的，从而在控制关系中，y中的元素只影响IMU反馈的姿态角度φ_i，与其他的运动无关。

根据简化后的稳像方法控制律，即公式(31)，进一步变换可以得到控制律u的求解如下：

至此，已经建立控制律和新输入向量y，以及关节角度与角速度、IMU角度与角速度之间的关系，从而将稳像控制问题简化为寻找控制输入向量y的问题。

为避免多个下标带来的误解，忽略表示IMU的下标i的使用，构造控制输入向量y如下：

公式中，K_p为比例调节系数矩阵，K_i为积分调节系数矩阵，φ_d表示期望的姿态角度，结合实时反馈的姿态角度φ_i，可以得到如下的关系：

φ_e＝φ_d-φ_i (34)

将公式(33)带入到公式(32)中，即可求得控制律u，控制律u将作为稳像方法的输出提供给驱动器的控制中，完成稳像方法控制，得到最终的控制律如下：

(4)稳像方法收敛性证明

公式(32)表明，结合构造的控制输入向量y，通过正逆运动学和速度运动学转成关节角速度，进而最终控制关节电机的运动，完成稳像控制。而稳像方法是在任务空间中表述运动控制任务，提供给控制器的是IMU位姿的稳定流，稳像控制的目标是通过关节速度控制使仿生眼能够跟踪该轨迹。故稳像方法的收敛性先从单一方向考虑，进而推广到三个方向(滚转、俯仰和偏航)。

根据公式(26)-(27)可以得到控制输入向量y和反馈的姿态角度φ_i之间的关系，并根据公式(33)所给出的控制输入向量y的表达式，可以得到如下的关系：

公式(34)给出了姿态角期望值、反馈值和误差值之间的关系，将这个公式等号左右两边均对时间微分可以得到：

从而将公式(37)带入到公式(36)中可以得到：

而公式(38)中存在积分项，不易进行收敛性证明，同样的，在等号左右两边同时对时间微分可以得到：

从而根据公式(39)进行基于扰动解耦与补偿的稳像方法的收敛性证明。

(5)单一方向上的算法收敛性

考虑单个方向上的稳像控制，有如下的表达形式：

可以将其写成标准的二阶形式如下：

公式中，ω_n被称为自然频率，ζ称为阻尼比，其中

参考质量-弹簧-阻尼系统，可以构造特征多项式：

s²+2ζω_ns+ω_n ²＝0 (42)

这个特征多项式的两个根分别为：

当且仅当ζω_n＞0、

时，二阶误差动力学(41)是稳定的。

当误差动力学稳定之后，对于二阶误差动力学(41)的解φ_e(t)有3种类型，这取决于根s_1,2是不等的两个实数(ζ＞1)或者s_1,2是相等的两个实数(ζ＝1)或者s_1,2是两个共轭复数(ζ＜1)。三种不同情况下的示例根位置如图6(a)、6(b)、6(c)所示，对应的φ_e(t)求解如下：

①过阻尼：ζ＞1，两个根s_1,2同为实数且不相等，二阶误差动力学(41)的解是：

其中c_1,2可以通过初始条件φ_e(0)和

计算得到；响应(即方程解)是两个衰减指数之和，时间常数分别为t₁＝-1/s₁和t₂＝-1/s₂。

②临界阻尼：ζ＝1，根s_1,2＝-ω_n为相等的两个实数，得到二阶误差动力学(41)的解是：

响应是一个衰减的指数函数乘以一个时间的线性函数，其中指数函数衰减的时间常数为t＝1/ω_n。

③欠阻尼：ζ＜1，根s_1,2是一对共轭复数s_1,2＝-ζω_n±jω_d，其中

是有阻尼固有频率，二阶误差动力学(41)的解是：

响应是一个衰减的指数函数乘以一个正弦函数，其中指数函数衰减的时间常数为t＝1/ω_n。

图7中给出过阻尼、临界阻尼和欠阻尼三种情况下的误差响应。并在图8中给出了根的位置和瞬态响应属性之间的关系：复平面中越靠近左侧的根，其对应的调节时间越短；而距离实轴越远的根，其对应的超调和振荡也就越大。值得注意的是，如果期望角度微分

不是常数，则控制器无法完全消除稳态误差。

(6)稳像方法的收敛性

对于基于扰动解耦与补偿的稳像方法，最终需要考虑到三个方向上的控制，而前述的单个方向的控制可以推广到三个方向上的控制。参考角度(期望姿态角)和实际角度现在变为一个3维向量，增益k_p和k_i现在变为一个3×3形式的矩阵K_p和K_i，K_p和K_i的形式如下：

从而公式(39)的稳定性分析与公式(40)-(46)单个方向上的稳定性分析相同。

(7)基于扰动解耦与补偿的稳像方法控制框架

本发明的基于扰动解耦与补偿的稳像方法控制框架如图9所示。装配于仿生眼颈部关节末端执行器IMU提供姿态角度φ_i和角速度ω_i反馈，关节编码器提供关节角度θ和角速度

反馈。根据期望的姿态角度φ_d和其微分量

进入到以PI控制器为基础的稳定化线性控制器中，得到控制输入向量y；然后结合IMU的各项反馈的姿态角度φ_i、关节编码器反馈关节角度θ和角速度

进行正逆运动学和速度运动学的处理，完成非线性补偿与解耦，最终得到稳像方法的控制律u；得到的控制律u将作为颈部关节电机运动的参考值，通过驱动器的控制关节电机运动，完成基于扰动解耦与补偿的稳像方法控制。

根据如上所述，基于扰动解耦与补偿的稳像算法如下：

所述实施例为本发明的优选的实施方式，但本发明并不限于上述实施方式，在不背离本发明的实质内容的情况下，本领域技术人员能够做出的任何显而易见的改进、替换或变型均属于本发明的保护范围。

Claims (6)

1.一种基于扰动解耦与补偿的仿生眼稳像方法，其特征在于：

稳定化线性控制器根据IMU反馈的姿态角度φ_i、期望的姿态角度φ_d和其微分量

得到控制输入向量y；

所述控制输入向量y为：

其中，K_p为比例调节系数，K_i为积分调节系数，φ_e＝φ_d-φ_i；

结合IMU的各项反馈的姿态角度φ_i、关节编码器反馈关节角度θ和角速度

进行正逆运动学和机器人速度运动学的计算，完成非线性补偿与解耦，最终得到稳像方法的控制律u；

所述控制律u为：

其中，V^-1＝(^WR₀ ⁰J_O)^-1，^wR₀为世界坐标系到坐标系0的旋转矩阵，雅克比矩阵

为雅克比矩阵；^WR_i ⁱω_i等于IMU坐标系相对世界坐标系的角速度^Wω_i；

所述IMU坐标系相对世界坐标系的角速度^Wω_i为：

其中，^Wω₀为0坐标系相对世界坐标系的角速度矢量。

2.根据权利要求1所述的基于扰动解耦与补偿的仿生眼稳像方法，其特征在于，还包括：

控制律u将作为颈部关节电机运动的参考值，通过驱动器控制关节电机运动，完成基于扰动解耦与补偿的稳像方法控制。

3.根据权利要求1所述的基于扰动解耦与补偿的仿生眼稳像方法，其特征在于，还包括根据

进行基于扰动解耦与补偿的稳像方法的收敛性证明。

4.根据权利要求1所述的基于扰动解耦与补偿的仿生眼稳像方法，其特征在于，所述正逆运动学的计算根据坐标系j和坐标系j-1的变换矩阵、IMU坐标系相对于世界坐标系的旋转矩阵和雅可比矩阵⁰J_O进行的；

所述坐标系j和坐标系j-1的变换矩阵为：

其中，α_j是z_j-1轴和z_j轴之间的夹角；a_j是使z_j-1经过坐标系j的原点时，沿x_j-1轴平移的距离；d_j是使x_j-1和x_j处于同一平面时，沿z_j-1轴平移的距离；θ_j是x_j-1轴和x_j轴之间的夹角；

所述IMU坐标系相对于世界坐标系的旋转矩阵为：

其中，R_x、R_y和R_z分别表示围绕x、y、z旋转的旋转矩阵，ψ、

和

分别为IMU绕x轴、y轴、z轴旋转的角度。

5.根据权利要求1所述的基于扰动解耦与补偿的仿生眼稳像方法，其特征在于，根据所述机器人速度运动学，建立IMU角速度、关节角速度和扰动角速度之间的关系为：

^Wω_i＝^WR₀ ⁰ω_i+^Wω₀。

6.一种实现权利要求1-5任一项所述的基于扰动解耦与补偿的仿生眼稳像方法的系统，其特征在于，包括：

稳定化线性控制器，用于根据IMU反馈的姿态角度φ_i、期望的姿态角度φ_d和其微分量

得到控制输入向量y；

非线性补偿与解耦器，用于结合IMU的各项反馈的姿态角度φ_i、关节编码器反馈关节角度θ和角速度

进行正逆运动学和速度运动学的计算，得到稳像方法的控制律u。

Images