一种基于仿生的机器人感知控制系统及控制方法与流程

本发明涉及无人机领域，具体涉及一种基于仿生的机器人感知控制系统及控制方法。

背景技术

随着机器人技术的快速发展，尤其是带有机械臂的机器人在搬运、抓取、分类等场景下取得了非常多的突破,这类具有与环境交互功能的机器人定位和导航技术成为各大公司和研究机构争相解决的重要问题。由于在大部分环境中飞行机械臂的运动和抓取必须在机器人本体保持静止或稳定状态下才能进行，这极大地限制了机器人的运动，其次目前大多数机器人的定位和导航模块都是固定在机器人本体身上，所以机器人的视野被迫随着机器人的运动而改变，这就导致了当机械臂在进行各种动作时机器人的视野保持固定而不能察觉到视野之外的环境，这也极大地限制了机器人对周围环境的感知能力，使得机器人难以应对不可预测的动态实际应用场景。

发明专利《一种基于仿生眼的无人驾驶车辆路径规划》(公开号：106529466a)。该发明公开了一种基于仿生眼的无人驾驶车辆路径规划方法，该发明中的仿生眼由电机进行控制可以进行上下运动、左右运动、绕光轴运动，同时仿生眼获取周围环境的图像，对图像进行sift特征点匹配以及harris角点匹配实现环境的三维重建，同时对目标车辆进行识别和跟踪获取目标车辆的位置和运动信息。该发明中将摄像头固定在车身上部利用电机来控制摄像头进行转动来增大视野的范围，其自由度有限相机本体并不能随意运动，因此该发明的应用范围非常有限。

发明专利《仿生眼定位追踪系统及其工作方法》(公开号：106042005a)。该发明公开了一种仿生眼的定位追踪方法，该发明中仿生眼通过仿生眼云台模块来实现，该云台可以实现摄像头的水平转动和垂直转动实现对周围环境的实时捕捉。该发明中的摄像头通过舵机控制关节固连于机器人上与上述发明专利类似，限制了摄像头的运动范围。

发明专利《仿生眼动眼控制系统及仿生眼动眼控制方法》(公开号：106155113a)。该发明公开了一种仿生眼动眼控制系统和仿生眼控制方法，该发明通过四个步进电机分别控制两个摄像头的俯仰和偏转，利用摄像头捕捉到的相邻两帧图像之差来计算目标在图像平面内的速度，根据目标的运动速度来控制四个步进电机的运动速度实现了模仿人眼实现扫视、平滑追踪的效果。该方法中仅摄像头的图像信息对摄像头的运动进行控制，而没有实现视觉系统所具有的高级定位导航等功能，不能满足机器人的视觉导航定位的重要需求。

技术实现要素：

为了解决上述技术问题，本发明目的之一是提供一种基于仿生的机器人感知控制系统，针对当前机器人定位导航系统固连于机器人本体之上导致机器人视野受限、机器人本体运动不够灵活等问题，通过模仿飞禽类视觉系统将机器人的视觉导航定位系统与机器人本体分离，实现机器人视野范围不受本体机器人自由度的约束，同时机器人本体的运动不受机械臂抓取的限制。

为实现上述发明目的，本发明采取的技术方案如下：

一种基于仿生的机器人感知控制系统，包括机器人本体、机械臂和视觉惯性导航模块，所述机器人本体上设置机械臂底座，所述机械臂安装于所述机械臂底座上，所述机械臂中设置多个旋转关节，每个所述旋转关节中设置一个舵机，所述视觉惯性导航模块安装在所述机械臂的末端，所述视觉惯性导航模块采用视觉惯性里程计进行实时定位并同步建立地图，所述视觉惯性里程计包括鱼眼摄像头和imu；

所述鱼眼摄像头用于捕捉图像；

所述imu通过测量三轴姿态角速度和加速度通过预积分得到所述imu的姿态信息，所述imu与所述视觉里程计采用紧耦合的融合方式将图像特征信息加入到系统的状态向量中进行联合优化，所述imu的姿态信息与所述视觉里程计的视觉信息融合用于更新姿态图并实时更新地图；

所述机械臂通过所述旋转关节调节所述机械臂末端的所述视觉惯性导航模块能够达到其工作空间中指定的任意位置和姿态，所述机器人通过所述视觉惯性导航模块在建立后的所述地图中进行导航和定位。

优选的，所述视觉惯性里程计为单目视觉惯性里程计，所述鱼眼摄像头为单目相机。

优选的，所述视觉惯性里程计为双目视觉惯性里程计，即在imu两侧各设置一个鱼眼摄像头，用于扩展视野。

优选的，所述机械臂具有六个自由度，每个自由度均由电机、减速齿轮组和编码器构成所述关节。减速齿轮组安装在电机的输出轴上，所述减速齿轮组中设置多个相互啮合的齿轮，减速齿轮组中的出轴齿轮上设置用于连接舵机中的舵盘的旋转轴。

优选的，所述舵机还包括舵机本体、舵机底座、副舵盘、主舵盘、设置在所述副舵盘和所述主舵盘之间沿所述舵机本体周向分布的舵机外侧壁，所述舵机本体设置在所述舵机底座上，所述副舵盘和主舵盘分别设置在所述舵机本体相对设置的顶壁和底壁的外侧。所述主舵盘和副舵盘设置相互对用的安装孔，所述安装孔用于固定传动连接件，从而使得副舵盘跟随主舵盘同步运动。

优选的，所述舵机包括第一舵机、第二舵机、第三舵机、第四舵机、第五舵机和第六舵机，所述第一舵机的副舵盘与所述机械臂底座相连接，所述第一舵机的主舵盘与所述第二舵机的舵机外侧壁连接，所述第二舵机的主舵盘与延长臂杆的一端固定侧相连，所述延长臂杆的另一端与所述第三舵机的舵机底座连接，所述第三舵机的主舵盘和副舵盘分别与两个互相平行的连杆的一端相连，两个互相平行的连杆的另一端分别固定在所述第四舵机的舵机外侧壁上，所述第四舵机的主舵盘与所述第五舵机的舵机底座相连接，所述第五舵机的主舵盘和副舵盘分别与两个互相平行的连杆的一端相连，两个互相平行的连杆的另一端与所述第六舵机的舵机外侧壁相连，所述第六舵机的主舵盘与所述视觉惯性导航模块连接。舵机通过电机控制器输出pwm波进行驱动，电机控制器通过调节pwm波的占空比来调节电机的旋转角度，并且每个电机都有编码器读取角速度值，从而能够实现每个关节的反馈控制。

优选的，所述舵机中设置旋转轴用于连接所述电机和主舵盘，所述第三舵机的旋转轴与所述第四舵机的旋转轴的方向相交，所述第四舵机的旋转轴与所述第一舵机的旋转轴的方向相交，所述第四舵机的旋转轴与所述第六舵机的旋转轴方向相同。

本发明的目的之二是提供一种基于仿生的机器人感知控制系统的控制方法，通过使用机械臂的末端的视觉惯性导航模块控制机器人本体实现运动，实现机器人本体在定位导航中的位置准确性，包括如下步骤：

包括如下步骤：

步骤(1).机器人通过视觉惯性导航模块在所需环境中遍历一次建立定位导航所需要的地图，所述地图中存储特征点和描述子；

步骤(2).机器人通过机械臂末端的鱼眼摄像头捕捉周围环境中信息与已经建好的地图中的关键特征点进行匹配得到鱼眼摄像头在当前地图中的位置信息；

步骤(3).确定当前状态下机械臂的状态使机械臂末端和机体之间就形成固连关系，根据固连关系计算出鱼眼摄像头和机器人本体之间的旋转平移关系，利用摄像头与机器人本体之间的旋转平移关系计算出机器人本体在当前地图中的位置信息，计算出机器人本体在当前地图坐标系中的位置信息后就可以根据位置信息来控制机器人达到目标位置。

优选的，所述步骤(1)中建立地图的过程为slam过程，slam过程包括前端和后端两个部分：

s1.视觉里程计通过鱼眼摄像头捕捉的图像进行特征点检测和匹配，同时判断当前帧是否为关键帧，根据八点法计算出图像的旋转和平移关系，然后通过三角化部分将图像在当前帧的特征点三维信息恢复出来生成3d点；

s2.如果当前帧是关键帧则进入后端优化部分，imu通过测量三轴姿态角速度和加速度并进行预积分得到所述imu的姿态信息，所述imu与所述视觉里程计采用紧耦合的融合方式，将图像特征信息加入到系统的状态向量中进行联合优化，用预积分技术来处理imu数据，将imu的运动模型的参考系由固定的初始参考系转为不断变化的相对参考系，所述机械臂的姿态信息与所述视觉里程计的视觉信息融合用于更新姿态图进行全局优化，并实时更新地图，所述视觉里程计在对近似图像进行特征点检测和匹配过程中通过回环检测更新所述姿态图，从而更新地图。

利用鱼眼摄像头和imu作为仿生机器人的视觉导航定位系统，通过imu信息恢复了单目场景下的地图尺度信息，同时在视觉惯性里程计的基础上实现了机器人中的无人飞行机械臂的多自由度定位。

优选的，所述步骤(3)中机器人本体要达到目标位置的步骤如下：

a1.通过视觉里程计获得机械臂末端的鱼眼摄像头相对于世界坐标系的姿态，包括平移和旋转鱼眼摄像头和机械臂的外参通过手眼标定的方法得到；

a2.机械臂固连于机器人本体下方，由机械臂的运动学关系可以得到机械臂末端的鱼眼摄像头相对于机体的姿态包括平移和旋转

a3.通过三维空间内的坐标系变换得到机器人本体相对于世界坐标系的姿态，包括平移和旋转其中

优选的，所述步骤(3)中机器人本体要达到目标位置的计算过程如下：

t＝(x,y,z)^t

其中表示机器人本体在世界坐标系下的位置，表示鱼眼摄像头在机器人机体坐标系下的位置，表示鱼眼摄像头在世界坐标系下的位置，r表示三维空间中的旋转，r、p、l分别表示绕x、y、z三个轴旋转的角度，t表示三维空间中的平移，x,y,z分别表示沿x、y、z三个轴的平移。

相对于现有技术，本发明取得了有益的技术效果：

本发明基于仿生的机器人感知控制系统及控制方法采用机械臂充当仿生机器人的脖子，视觉惯性导航模块固连在机械臂的末端充当仿生机器人的视觉系统，最大限度的增大视觉系统的运动范围，增强机器人本体的灵活性以应对各种动态场景。本发明中定位导航系统采用单目视觉惯性里程计结合实时建图方案，同时利用生成的栅格地图进行实时的导航和定位。通过视觉惯性导航模块作为仿生机器人的视觉导航定位系统，利用机械臂的多自由度运动使得机器人视觉系统摆脱机器人本体运动的限制，极大地增加了机器人的视野范围使机器人能够应对复杂多变的动态场景，同时该视觉系统利用视觉惯性slam技术实现实时的导航和定位功能，使仿生机器人具有很强的人机交互功能，可以应用在各种动态场景下，具有很大的应用价值。

附图说明

图1为本发明基于仿生的机器人感知控制系统的结构示意图；

图2为本发明基于仿生的机器人感知控制系统的控制方法中slam过程的流程图；

图3为本发明基于仿生的机器人感知控制系统的控制方法中机器人视觉控制流程图。

附图标记：

1.螺旋桨；2.螺旋桨电机；3.本体机架；4.电池；5.机械臂底座；6.第一舵机；7.第二舵机；8.延长臂杆；9.第三舵机；10.第四舵机；11.第五舵机；12.第六舵机；13.鱼眼摄像头；14.相机支架；15.连杆。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合实施例对本发明进行进一步详细说明，但本发明要求保护的范围并不局限于下述具体实施例。

系统实施例

如图1和2所示，一种基于仿生的机器人感知控制系统，包括机器人本体、机械臂和视觉惯性导航模块，所述机器人本体上设置机械臂底座5、若干个均匀分布的螺旋桨1、控制螺旋桨1旋转的螺旋桨电机2、安装螺旋桨1的本体机架3和电池4，电池4安装在机械臂底座5的上表面，所述机械臂安装于所述机械臂底座5的下表面，通过控制螺旋转实现机器人的飞行，所述机械臂中设置多个旋转关节，每个旋转关节对应一个自由度，每个所述旋转关节中设置一个舵机。所述机械臂通过所述旋转关节调节所述机械臂末端的所述视觉惯性导航模块能够达到其工作空间中指定的任意位置和姿态，所述机器人通过所述视觉惯性导航模块在建立后的所述云地图中进行导航和定位。

所述机械臂具有六个自由度，每个自由度均由电机、减速齿轮组和编码器构成所述舵机。减速齿轮组安装在电机的输出轴上，所述减速齿轮组中设置多个相互啮合的齿轮，减速齿轮组中的出轴齿轮上设置用于连接舵机中的舵盘的旋转轴。所述舵机还包括舵机本体、舵机底座、副舵盘、主舵盘、设置在所述副舵盘和所述主舵盘之间沿所述舵机本体周向分布的舵机外侧壁，所述舵机本体设置在所述舵机底座上，所述副舵盘和主舵盘分别设置在所述舵机本体相对设置的顶壁和底壁的外侧。所述主舵盘和副舵盘设置相互对用的安装孔，所述安装孔用于固定传动连接件，从而使得副舵盘跟随主舵盘同步运动。

所述舵机包括第一舵机6、第二舵机7、第三舵机9、第四舵机10、第五舵机11和第六舵机12，所述第一舵机6的副舵盘与所述机械臂底座5相连接，所述第一舵机6的主舵盘与所述第二舵机7的舵机外侧壁连接从而带动第二舵机7转动。所述第二舵机7的主舵盘与延长臂杆8的一端固定侧相连从而带动延长臂杆8转动，所述延长臂杆8的另一端与所述第三舵机9的舵机底座连接，延长臂杆8转动的同时带动第三舵机9转动，所述第三舵机9的主舵盘和副舵盘分别与两个互相平行的连杆15的一端相连，两个互相平行的连杆15的另一端分别固定在所述第四舵机10的舵机外侧壁上，通过连杆15实现第三舵机9的主舵盘和副舵盘带动第四舵机10转动。所述第四舵机10的主舵盘与所述第五舵机11的舵机底座相连接从而带动第五舵机11转动，所述第五舵机11的主舵盘和副舵盘分别与两个互相平行的连杆15的一端相连，两个互相平行的连杆15的另一端与所述第六舵机12的舵机外侧壁相连，通过连杆15实现第五舵机11的主舵盘和副舵盘带动第六舵机12转动。所述第六舵机12的主舵盘与所述视觉惯性导航模块通过相机支架14连接，所述第六舵盘的主舵盘带动相机支架14上的视觉惯性导航模块转动。所述舵机中设置的旋转轴用于连接所述电机和主舵盘，所述第三舵机9的旋转轴与所述第四舵机10的旋转轴的方向相交，所述第四舵机10的旋转轴与所述第一舵机6的旋转轴的方向相交，所述第四舵机10的旋转轴与所述第六舵机12的旋转轴方向相同，所述第一舵机6的旋转轴与所述第二舵机7的旋转轴的方向相交，所述第二舵机7的旋转轴与所述第三舵机9的旋转轴的方向相同，所述第三舵机9的旋转轴与所述第五舵机11的旋转轴的方向相同。

舵机通过电机控制器输出pwm波进行驱动，电机控制器通过调节pwm波的占空比来调节电机的旋转角度，并且每个电机都有编码器读取角速度值，从而能够实现每个关节的反馈控制。六个舵机形成六个自由度，使机械臂能够达到其工作空间中指定的任意位置和姿态。采用机械臂充当仿生机器人的脖子，视觉惯性导航模块固连在机械臂的末端充当仿生机器人的视觉系统，最大限度的增大视觉系统的运动范围，增强机器人本体的灵活性以应对各种动态场景。

所述视觉惯性导航模块安装在所述机械臂的末端，所述视觉惯性导航模块采用视觉惯性里程计进行实时同步建立云地图，所述视觉惯性里程计包括鱼眼摄像头13、imu；所述鱼眼摄像头13用于捕捉图像，鱼眼摄像头13作为捕捉图像的传感器；视觉里程计依据所述鱼眼摄像头13捕捉的图像序列得到当前帧的三维信息生成3d点，所述视觉里程计通过特征点法得到当前帧的三维信息并确定所述机器人的位置和姿态；所述imu是惯性测量单元，是用于测量物体三轴姿态角(或角速率)以及加速度的装置，所述imu通过测量三轴姿态角和加速度并进行预积分得到所述imu的姿态信息，所述imu与所述视觉里程计采用紧耦合的融合方式将图像特征信息加入到系统的状态向量中进行联合优化，所述机械臂的姿态信息与所述视觉里程计的视觉信息融合用于更新姿态图并实时更新云地图。所述视觉惯性里程计为单目视觉惯性里程计，所述鱼眼摄像头13为单目相机；所述视觉惯性里程计也可以为双目视觉惯性里程计，即在imu两侧各设置一个鱼眼摄像头，用于扩展视野。

控制方法实施例：

如图2和3所示，本发明的目的之二是提供一种基于仿生的机器人感知控制系统的控制方法，通过使用机械臂的末端的视觉惯性导航模块控制机器人本体实现运动，实现机器人本体在定位导航中的位置准确性，包括如下步骤：

包括如下步骤：

步骤(1).机器人通过视觉惯性导航模块在所需环境中遍历一次建立定位导航所需要的地图，所述地图中存储特征点和描述子；

步骤(2).机器人通过机械臂末端的鱼眼摄像头13捕捉周围环境中信息与已经建好的地图中的关键特征点进行匹配得到鱼眼摄像头13在当前地图中的位置信息；

步骤(3).确定当前状态下机械臂的状态使机械臂末端和机体之间就形成固连关系，根据固连关系计算出鱼眼摄像头13和机器人本体之间的旋转平移关系，利用摄像头与机器人本体之间的旋转平移关系计算出机器人本体在当前地图中的位置信息，计算出机器人本体在当前地图坐标系中的位置信息后就可以根据位置信息来控制机器人达到目标位置。

优选的，所述步骤(1)中建立地图的过程为slam过程，slam过程包括前端和后端两个部分：

s1.视觉里程计通过鱼眼摄像头13捕捉的图像进行特征点检测和匹配，根据八点法计算出图像的旋转和平移关系，然后通过三角化部分将图像在当前帧的特征点三维信息恢复出来生成3d点，之后继续判断当前帧是否为为关键帧；三角化部分采用直接线性变化，利用同一个特征点在不同两帧下的投影坐标，求出该特征点在世界坐标系下的3d坐标；

s2.如果当前帧是关键帧则进入后端优化部分，imu通过测量三轴姿态角速度和加速度并进行预积分得到所述imu的姿态信息，所述imu与所述视觉里程计采用紧耦合的融合方式，将图像特征信息加入到系统的状态向量中进行联合优化，状态向量是状态变量在某一时刻的值，即为系统在时刻的状态。状态变量在t＝0时刻的值称为系统的初始状态或起始状态，即也称为初始状态向量或起始状态向量。同时用预积分技术来处理imu数据，将imu的运动模型的参考系由固定的初始参考系转为不断变化的相对参考系，所述机械臂的姿态信息与所述视觉里程计的视觉信息融合用于更新姿态图进行全局优化，并实时更新地图，所述视觉里程计在对近似图像进行特征点检测和匹配过程中通过回环检测更新所述姿态图，从而更新地图。利用鱼眼摄像头13和imu作为仿生机器人的视觉导航定位系统，通过imu信息恢复了单目场景下的地图尺度信息，同时在视觉惯性里程计的基础上实现了机器人中的无人飞行机械臂的多自由度定位。

当机器人在飞行过程中捕捉到相似度很高的两帧图像时，根据特征匹配，比较匹配的数量是否足够多，一般通过采用词袋模型，用来加速特征匹配，如果匹配特征足够多，则更新3d点从而更新云地图。

所述步骤(3)中机器人本体要达到目标位置的步骤如下：

a1.通过视觉里程计获得机械臂末端的鱼眼摄像头13相对于世界坐标系的姿态，包括平移和旋转鱼眼摄像头13和机械臂的外参通过手眼标定的方法得到；

a2.机械臂固连于机器人本体下方，由机械臂的运动学关系可以得到机械臂末端的鱼眼摄像头13相对于机体的姿态包括平移和旋转

a3.通过三维空间内的坐标系变换得到机器人本体相对于世界坐标系的姿态，包括平移和旋转其中

优选的，所述步骤(3)中机器人本体要达到目标位置的计算过程如下：

t＝(x,y,z)^t

其中表示机器人本体在世界坐标系下的位置，表示鱼眼摄像头在机器人机体坐标系下的位置，表示鱼眼摄像头在世界坐标系下的位置，r表示三维空间中的旋转，r、p、l分别表示绕x、y、z三个轴旋转的角度，t表示三维空间中的平移，x,y,z分别表示沿x、y、z三个轴的平移。

本发明中定位导航系统采用单目视觉惯性里程计结合实时建图方案，同时利用生成的栅格地图进行实时的导航和定位。通过视觉惯性导航模块作为仿生机器人的视觉导航定位系统，利用机械臂的多自由度运动使得机器人视觉系统摆脱机器人本体运动的限制，极大地增加了机器人的视野范围使机器人能够应对复杂多变的动态场景，同时该视觉系统利用视觉惯性slam技术实现实时的导航和定位功能，使仿生机器人具有很强的人机交互功能，可以应用在各种动态场景下，具有很大的应用价值。

根据上述说明书的揭示和教导，本发明所属领域的技术人员还可以对上述实施方式进行变更和修改。因此，本发明并不局限于上面揭示和描述的具体实施方式，对发明的一些修改和变更也应当落入本发明的权利要求的保护范围内。此外，尽管本说明书中使用了一些特定的术语，但这些术语只是为了方便说明，并不对发明构成任何限制。