具有模仿学习机制的手把手示教机械臂系统及方法，所述系统包括机械手臂和舵机组成的动作执行模块、多传感器组成的感知模块以及控制器构成的学习模块。本发明采用模块独立供电方式，在此基础上使用手把手离线示教，将一组由陀螺仪传感器和加速度计组成的动作检测装置作为感知模块搭载在机械臂上，采集机械臂在运动过程中的各连杆的状态信息，然后将这些信息应用模仿学习算法，指导机械臂系统模仿学习示教行为。本发明采取分级分层控制，提升了控制系统的针对性和信息传递效率，本发明能够通过学习获知示教行为的目的，在改变机械臂初始姿态或目标物体所在方位时依旧能够完成模仿任务，具有较高的智能程度。
1.具有模仿学习机制的手把手示教机械臂系统，其特征在于：该系统中，DSP控制器(2)、7.4v锂电池(5)、电压转化器(6)、长U型连杆一(7)固定安装在机械臂系统底盘(1)上；
51单片机最小系统(3)、舵机控制器(4)分别与DSP控制器(2)连接；所述电压转化器(6)与
7.4v锂电池(5)连接，7.4v锂电池(5)与DSP控制器(2)连接；长U型连杆一(7)的侧面安装有长U型连杆二(8)；长U型连杆二(8)的一端安装有超声波传感器(18)；
两个长U型连杆的底部连接组成H型连杆(9)；H型连杆(9)一端与长U型连杆二(8)的侧面连接；长U型连杆三(10)一端与H型连杆(9)另一端连接；机械臂手爪(11)与长U型连杆三(10)的另一端连接；
MG996R舵机一(12)安装在长U型连杆二(8)与长U型连杆一(7)的连接处；
MG996R舵机二(13)安装在H型连杆(9)与长U型连杆二(8)的侧面连接处；
MG996R舵机三(14)安装在H型连杆(9)与长U型连杆三(10)的一端连接处；
MG996R舵机四(15)安装在长U型连杆三(10)的另一端部处；
MG996R舵机五(16)安装在机械臂手爪(11)的端部；
MG996R舵机六(17)安装在机械臂手爪(11)的侧面；
三轴陀螺仪传感器一(19)与加速度计模块一(22)组成检测模块一，检测模块一设置在H型连杆(9)中间位置处；
三轴陀螺仪传感器二(20)与加速度计模块二(23)组成检测模块二，检测模块二设置在长U型连杆三(10)中间位置处；
三轴陀螺仪传感器三(21)与加速度计模块三(24)组成检测模块三，检测模块三设置在机械臂手爪(11)的一端；
红外测距传感器(25)设置在MG996R舵机六(17)的中间位置；
触觉传感器一(26)、触觉传感器二(27)分别设置在机械臂手爪(11)的上下手爪处；
具有模仿学习机制的手把手示教机械臂系统方法，机械臂系统工作时，首先采用手把手示教方法对机械手臂进行示教，由感知模块采集示教信息后经控制系统进行模仿学习，控制机械臂模仿示教行为动作；
当机械臂系统启动后，系统的舵机控制器不上电，即机械手臂上舵机处于断电状态，采用离线示教的方式，仅感知模块进行工作；用手拖动机械臂完成夹取桌面上物体的动作，三个三轴陀螺仪传感器和加速度计模块组成的动作检测装置采集在示教过程中三个手臂连杆的位姿坐标旋转角速度和角加速度信息，红外测距传感器采集手爪到目标物体的距离信息；动作检测装置将采集到的示教行为信号和距离信号通过IO口传送给DSP控制器，DSP控制器将采集到的信号进行处理后，获得示教行为的状态信息；
为51单片机最小系统和舵机控制器上电，即机械手臂上舵机处于通电状态；系统首先通过单片机使机械臂还原到手把手示教前所处的姿态，然后系统通过搭载的传感器搜寻确定目标物体的方位，底座舵机搭载着的超声波传感器进行旋转确定目标物体所在方向，手爪方向的舵机进行旋转使其搭载的红外测距传感器与目标物体保持正对方向，机械臂保持此姿态；
机械臂系统进入模仿学习阶段，构建前馈神经网络，将示教行为的状态信息作为网络的输入，机械臂运动的动作策略作为网络的输出；DSP控制器将获得的动作策略传送给51单片机最小系统，由单片机控制舵机控制器将关节角转化为脉宽信号以控制机械臂上的关节转动相应角度，使手爪向目标物体方向运动；机械臂运动过后由红外测距传感器采集手爪与目标物体的距离信息并传送给DSP控制器，若手爪与目标物体距离大于0cm或检测不到目标物体，则继续进行模仿学习，重新调整动作策略，产生舵机控制信号，循环运行，直至满足设定的模仿结束条件即红外测距传感器检测到手爪与目标物体距离为0cm，手爪闭合，由机械臂手爪处的触觉传感器检测是否夹取到目标物体，从而完成示教行为动作。
2.根据权利要求1所述的具有模仿学习机制的手把手示教机械臂系统，其特征在于：首先对机械臂进行手把手示教，由感知模块采集示教行为信息并进行处理，然后根据目标物体位置确定机械臂初始状态，系统进行模仿学习并再现示教行为动作；所述机械臂系统模仿学习的方法包括以下步骤：步骤1，手把手示教及示教数据处理；
此阶段电源模块仅为DSP控制器(2)、三轴陀螺仪传感器一(19)、三轴陀螺仪传感器二(20)、三轴陀螺仪传感器三(21)和加速度计模块一(22)、加速度计模块二(23)、加速度计模块三(24)供电，其它元器件处于断电状态；对机械臂采取手把手离线示教方式，拖拽机械臂夹取目标物体，由感知模块采集示教行为信息；具体包括以下步骤：步骤1.1，初始化机械臂系统；
配置DSP控制器(2)寄存器状态，初始化全局和局部变量，初始化传感器的位置和舵机的控制量；电源模块仅为DSP控制器(2)、三轴陀螺仪传感器和加速度计模块供电，其它元器件处于断电状态；
步骤1.2，对机械臂采用手把手离线示教的方式，拖拽机械臂使机械臂手爪(11)夹住与机械臂系统底盘(1)在同一平面上的目标物体，拖拽时保持手爪对向目标物体；在此过程中，由三轴陀螺仪传感器和加速度计模块组成的动作检测装置采集所在连杆位姿坐标的角速度和角加速度，由红外测距传感器采集到目标物体的距离；为保证示教的质量，使传感器能够精确地采集信号，以较缓慢的速度手把手拖拽机械臂手爪夹取目标物体，全程所用时间控制在5s-10s，并采取3-5次示教行为；
步骤1.3，DSP控制器(2)将采集到的位姿坐标角速度和角加速度进行数据处理并通过自适应加权融合算法进行融合，将采集到的距离转化成手爪到目标物体距离，从而得到示教行为状态信息；
陀螺仪用于测量连杆角速度信号,通过对角速度积分,便能得到角度值；每个三轴陀螺仪传感器每1ms采集一个数据，每10个值进行一次数据处理，由于时间很短，忽略时间差异带来的误差，三轴陀螺仪传感器得到的角度计算为：angleAn＝angleAn-1+gyron×dt
其中angleAn为陀螺仪第n次采样到的角度值，angleAn-1为陀螺仪第n-1次采样到的角度值，gyron为陀螺仪第n次采样到的瞬时角速率值，dt为手把手示教过程所用时间；
加速度计用于测量连杆的线性加速度，加速度计的输出值与倾角呈非线性关系，随着倾角的增加而表现为正弦函数变化，同三轴陀螺仪传感器采集数据的方法，加速度计模块得到的角度计算为：angleB＝atan2(y,z)×(180/3.14)
其中定义加速度计模块x轴朝下，y轴朝前；angleB为加速度计获得的角度，atan2(y,z)为该倾角竖直方向的弧度；
取三轴陀螺仪传感器采集到的信号angleAn和加速度计模块采集到的信号angleB，采用自适应加权融合算法方法进行数据融合，它无需传感器测量数据的任何先验知识，即可融合出方差最小的数据融合值，估计后的方差小于单个传感器估计的方差和采用多传感器均值平均做估计的方差；机械臂系统各连杆的真实倾角状态信息sD计算公式如下：sD＝W1×angleAn+W2×angleB
其中，W1为三轴陀螺仪传感器的最优加权因子，W2为加速度计模块的最优加权因子，故示教行为状态信息sD＝(sD1,sD2,...,sDn)；
设红外测距传感器(25)检测到与目标物体距离为d，由于传感器发射口距离手爪夹取中心位置为7cm，故实际手爪到目标物体距离为L＝d-7cm；机械臂系统的示教及示教数据处理的阶段结束；
此阶段由系统搭载的超声波传感器(18)和红外测距传感器(25)搜寻目标物体所处的位置，确定机械臂的初始姿态，使系统在改变机械臂初始姿态或目标物体所在方位时，依旧能够完成任务；
配置DSP控制器(2)和51单片机最小系统(3)的寄存器状态，初始化全局和局部变量，初始化传感器的位置以及舵机和舵机控制器4的控制量；
51单片机最小系统(3)通过控制程序，使机械臂还原到手把手示教前的初始姿态，使机械臂在模仿学习示教行为之前，各连杆保持与手把手示教前相同的位姿坐标，以确保即使改变机械臂初始姿态系统仍能完成模仿学习任务；
51单片机最小系统(3)通过控制程序，使MG996R舵机一(12)从左向右进行180°旋转，搜寻与系统处于同一桌面的目标物体，当MG996R舵机一(12)所在连杆上的超声波传感器(18)检测到正前方的目标物体时，MG996R舵机一(12)停止转动且固定在当前位置，确定了目标物体所在方向，以确保即使改变目标物体所在方位时系统仍能完成模仿学习任务；
由于红外测距传感器(25)安装在与机械臂手爪平行位置，红外线发射方向为手爪爪尖方向，因此51单片机最小系统(3)通过控制程序，使MG996R舵机四(15)向下转动直到红外测距传感器正对向目标物体，即手爪与目标物体处于正对方向，机械臂系统确定初始状态的阶段结束；
此阶段通过模仿学习算法由DSP控制器(2)获得动作策略，51单片机最小系统(3)通过动作策略控制机械臂进行运动，模仿示教行为动作，通过红外测距传感器(25)判定是否完成模仿任务，循环运行，直到红外测距传感器检测到手爪到达目标物体后，机械臂手爪闭合，完成示教行为的模仿；具体包括以下步骤：步骤3.1，初始化机械臂系统；
配置DSP控制器(2)和51单片机最小系统(3)的寄存器状态，初始化全局和局部变量，初始化传感器的位置以及舵机和舵机控制器(4)的控制量；
步骤3.2，对机械臂系统使用模仿学习算法，采用前馈神经网络的方法获得控制策略，以由陀螺仪和加速度计在示教行为中采集的连杆某时刻状态sD＝(sD1,sD2,...,sDn)作为神经网络的输入层，输入层有n个神经元；以h＝(h1,h2,...,hp)为隐含层的输出，输入层与隐含层的连接权值为wj＝(wj1,wj2,...,wjp)，隐含层神经元阈值为bj＝(bj1,bj2,...,bjp)，隐含层传递函数为f1(·)，隐含层有p个神经元；以相应时刻的动作策略a＝(a1,a2,...,aq)作为神经网络的输出层，隐含层与输出层的连接权值为wk＝(wk1,wk2,...,wkq)，输出层神经元阈值为bk＝(bk1,bk2,...,bkq)，输出层传递函数为f2(·)，输出层有q个神经元；
(1)选取t个输入样本，即t时刻连杆的状态作为输入sD(t)＝(sD1(t),sD2(t),...,sDn(t))，根据BP神经网络原理，采用FR共轭梯度法训练网络，可以得到网络隐含层输出为：
(2)利用输出层各神经元的误差偏导数δ(t)和网络隐含层各神经元的输出h(t)来修正连接权值wk(t)：
其中wkN+1为更新后的连接权值，wkN为更新前的连接权值，η为学习率；
(3)最终得到神经网络输出t时刻的动作策略表示为：
步骤3.3，DSP控制器将训练得到动作策略a(t)传输给单片机最小系统，由单片机控制舵机控制器使机械臂进行运动，模仿手爪到目标物体的示教行为动作；机械臂执行完动作后，由红外测距传感器检测与目标物体的距离信息并传送给DSP控制器，即检测手爪是否到达目标物体；若红外测距传感器检测到手爪与目标物体距离大于0cm或没有检测到目标物体，说明单片机系统执行的模仿学习动作未能使手爪到达目标物体或偏离目标物体，则此动作策略失败，系统重新回到步骤3.2继续进行模仿学习获得新的动作策略，直到单片机最小系统通过满足设定模仿结束条件的动作策略a'(t)使机械臂手爪到达目标物体位置，即红外传感器检测手爪到目标物体的距离信息L＝0cm时，机械臂停止运动；
步骤3.4，此时通过单片机控制舵机17开始转动，转动方向为使机械臂手爪闭合方向，当贴在手爪夹取位置的触觉传感器一(26)、触觉传感器二(27)检测到信号时说明手爪已经夹住目标物体，MG996R舵机六(17)停止转动，机械臂系统实现对目标物体的夹取，至此完成了对全部示教行为的模仿，机械臂系统模仿学习示教行为的阶段结束，从而整个机械臂系统工作结束。
具有模仿学习机制的手把手示教机械臂系统及方法
技术领域
[0001] 本发明涉及一种具有模仿学习机制的手把手示教机械臂系统及方法，属于智能机器人范畴。
背景技术
[0002] 机器人发展至今已经相当可观地影响着现代人类生活的诸多方面，人们对机器人的智能要求不断提高以便其更好地为人类服务，机器人学习是提高机器人智能化程度的一种行之有效的方法。自然界中，模仿是人或动物掌握运动技能最直接有效的学习方式。而人与动物是机器人的仿生原型，因此将模仿的机制应用于机器人是可行的。人或动物的诸多技能或行为是在其神经系统认知过程中渐进地形成和发展起来的，通过模仿人或动物，使机器人的认知和行为更加接近人或动物，从而产生自主的行为，这个过程从学习方法上称为模仿学习。
[0003] 模仿学习使机器人通过观察示教者的动作就可以快速学习到有用的动作。这种在与环境的交流中学习新知识和解决问题的方法，使得模仿学习与传统的孤立的机器人学习方法相比，模仿学习有很多优势：(1)学习效率高，直接获得有效信息，规避了庞大学习空间搜索问题；(2)提升智能，使机器人在日常环境下不断提升行为能力；(3)实用性强，学习后的行为可直接作用于工作环境；(4)增强了人机交互能力，使人机关系更加友好；(5)减少了编程的复杂程度。因此，模仿学习无论在军事领域还是民事领域都具有广泛的应用前景。模仿学习不仅是行为科学和神经科学的研究重点，对于人工智能和机器学习而言也具有非常重要的研究意义。
[0004] 目前，能够对示教动作进行再现的机器人以工业机器人为主，此类机器人只能基于示教经验通过对其编程，完成部分预设动作，智能程度较低，不具备模仿学习的能力。沈阳工业大学的杨俊友等人将模仿学习机制运用到机械臂上，通过采集视觉信息控制图模型完成示教动作的模仿，在模仿学习运用到实际机器人上取得一些突破。但这种机器人所用设备成本高，采集的视觉信息要通过进一步的数据处理，模仿不具备实时性，完成模仿任务耗时较长。
发明内容
[0005] 为了克服上述现有技术的不足，本发明提供了一种具有模仿学习机制的手把手示教机械臂系统及方法，通过改进的手把手示教方式直接拖拽操作机械臂，由一种动作检测装置感知示教行为动作信息后完成对示教动作的模仿学习，使机械臂系统不仅可以模仿再现示教动作，还能够通过学习获知示教行为的目的，具有较高智能程度。同时减小了传统手把手示教方式 的劳动强度，降低了传感器成本，克服了采用摄像技术采集示教行为带来的图像处理繁琐的问题，提高了机械臂系统的学习效率。
[0006] 本发明提出的机械臂系统，是一种具有模仿学习机制的机械臂系统，主要包括执行模块、感知模块和学习模块。系统执行模块包括六自由度机械臂及手爪；感知模块包括动作检测装置和目标物体检测装置；学习模块作为控制系统的大脑，完成传感器数据处理、舵机控制器的控制以及实现模仿学习算法等功能。本发明以机械臂手爪夹取桌面上物体的动作为例，使机械臂模仿学习该行为，以验证系统及方法的可行性。
[0007] 本发明解决其技术问题所采用的技术方案是：
[0008] 具有模仿学习机制的手把手示教机械臂系统，该系统中，DSP控制器(2)、7.4v锂电池(5)、电压转化器(6)、长U型连杆一(7)固定安装在机械臂系统底盘(1)上；51单片机最小系统(3)、舵机控制器(4)分别与DSP控制器(2)连接；所述电压转化器(6)与7.4v锂电池(5)连接，7.4v锂电池(5)与DSP控制器(2)连接；长U型连杆一(7)的侧面安装有长U型连杆二(8)；长U型连杆二(8)的一端安装有超声波传感器(18)；
[0009] 两个长U型连杆的底部连接组成H型连杆(9)；H型连杆(9)一端与长U型连杆二(8)的侧面连接；长U型连杆三(10)一端与H型连杆(9)另一端连接；机械臂手爪(11)与长U型连杆三(10)的另一端连接；
[0010] MG996R舵机一(12)安装在长U型连杆二(8)与长U型连杆一(7)的连接处；
[0011] MG996R舵机二(13)安装在H型连杆(9)与长U型连杆二(8)的侧面连接处；
[0012] MG996R舵机三(14)安装在H型连杆(9)与长U型连杆三(10)的一端连接处；
[0013] MG996R舵机四(15)安装在长U型连杆三(10)的另一端部处；
[0014] MG996R舵机五(16)安装在机械臂手爪(11)的端部；
[0015] MG996R舵机六(17)安装在机械臂手爪(11)的侧面；
[0016] 三轴陀螺仪传感器一(19)与加速度计模块一(22)组成检测模块一，检测模块一设置在H型连杆(9)中间位置处；
[0017] 三轴陀螺仪传感器二(20)与加速度计模块二(23)组成检测模块二，检测模块二设置在长U型连杆三(10)中间位置处；
[0018] 三轴陀螺仪传感器三(21)与加速度计模块三(24)组成检测模块三，检测模块三设置在机械臂手爪(11)的一端；
[0019] 检测模块一、检测模块二、检测模块三组成
[0020] 红外测距传感器(25)设置在MG996R舵机六(17)的中间位置；
[0021] 触觉传感器一(26)、触觉传感器二(27)分别设置在机械臂手爪(11)的上下手爪处。
[0022] 本发明所述的机械臂系统工作时，首先采用手把手示教方法对机械手臂进行示教，由感知模块采集示教信息后经控制系统进行模仿学习，控制机械臂模仿示教行为动作。
[0023] (1)手把手示教及示教数据处理
[0024] 当机械臂系统启动后，系统的舵机控制器不上电，即机械手臂上舵机处于断电状态，采用离线示教的方式，仅感知模块进行工作。用手拖动机械臂完成夹取桌面上物体的动作，三个三轴陀螺仪传感器和加速度计模块组成的动作检测装置采集在示教过程中三个手臂连杆的位姿坐标旋转角速度和角加速度信息，红外测距传感器采集手爪到目标物体的距离信息。动作检测装置将采集到的示教行为信号和距离信号通过IO口传送给DSP控制器，DSP控制器将采集到的信号进行处理后，获得示教行为的状态信息。
[0025] (2)机械臂系统确定初始状态
[0026] 为51单片机最小系统和舵机控制器上电，即机械手臂上舵机处于通电状态。系统首先通过单片机使机械臂还原到手把手示教前所处的姿态，然后系统通过搭载的传感器搜寻确定目标物体的方位，底座舵机搭载着的超声波传感器进行旋转确定目标物体所在方向，手爪方向的舵机进行旋转使其搭载的红外测距传感器与目标物体保持正对方向，机械臂保持此姿态。
[0027] (3)机械臂系统模仿学习示教行为
[0028] 机械臂系统进入模仿学习阶段，构建前馈神经网络，将示教行为的状态信息作为网络的输入，机械臂运动的动作策略作为网络的输出。DSP控制器将获得的动作策略传送给51单片机最小系统，由单片机控制舵机控制器将关节角转化为脉宽信号以控制机械臂上的关节转动相应角度，使手爪向目标物体方向运动。机械臂运动过后由红外测距传感器采集手爪与目标物体的距离信息并传送给DSP控制器，若手爪与目标物体距离大于0cm或检测不到目标物体，则继续进行模仿学习，重新调整动作策略，产生舵机控制信号，循环运行，直至满足设定的模仿结束条件(红外测距传感器检测到手爪与目标物体距离为0cm)，手爪闭合，由机械臂手爪处的触觉传感器检测是否夹取到目标物体，从而完成示教行为动作。
[0029] 与现有技术相比，本发明具有以下优点：
[0030] (1)本发明采用模块独立供电方式，为感知模块和执行模块分别独立供电，在此基础上使用一种改进的手把手离线示教的方法，这种方法能够克服手把手示教过程中关节的反力作用和劳动强度大的缺点，从而良好地完成示教任务，提高示教效率。
[0031] (2)本发明将一种由陀螺仪传感器和加速度计模块组成的动作检测装置搭载在机械臂上，作为系统的感知模块采集机械臂运动过程中的行为状态信息，然后对这些信息应用学习算法，指导机械臂系统模仿学习。使用这种采集机械臂运动行为信息的装置以及方法，能够降低传感器的成本，同时克服了摄像头技术后期复杂的图像处理过程，提高了机械臂系统模仿学习的效率。
[0032] (3)本发明控制系统采取分级分层控制的方法，相对独立地开展控制活动，通过DSP控制器进行对感知模块采集的数据的处理以及模仿学习算法的训练，通过51单片机最小系统运行动作策略，控制机械臂的运动，该方法提升了控制系统的针对性和信息传递效率。
[0033] (4)本发明将模仿学习机制应用于机械臂系统中，具有模仿学习机制的机械臂系统不仅可以再现示教动作，还能够通过学习获知示教行为的目的，在改变机械臂初始姿态或目标物体所在方位时依旧能够完成模仿任务，具有较高的智能程度。
附图说明
[0034] 图1.1为本发明所涉及系统的机械结构图；
[0035] 图1.2为本发明所涉及系统的整体结构图；
[0036] 图2为本发明所涉及系统的系统组成框图；
[0037] 图3为本发明所涉及系统运行过程流程图；
[0038] 图4为手把手示教及示教数据处理的方法流程图；
[0039] 图5为机械臂系统确定初始状态的方法流程图；
[0040] 图6为系机械臂统模仿学习示教行为的方法流程图；
[0041] 图7为系统模仿学习算法所使用的的前馈神经网络；
[0042] 图8.1为本发明所涉及系统主体前视图；
[0043] 图8.2为本发明所涉及系统上视图；
[0044] 图8.3为本发明所涉及系统右视图；
[0045] 图中：1-机械臂系统底盘，2-DSP控制器，3-51单片机最小系统，4-舵机控制器，5-7.4v锂电池，6-电压转化器，7-长U型连杆一，8-长U型连杆二，9-H型连杆，10-长U型连杆，
11-机械臂手爪，12-MG996R舵机一，13-MG996R舵机二，14-MG996R舵机三，15-MG996R舵机四，16-MG996R舵机五，17-MG996R舵机六，18-超声波传感器，19-三轴陀螺仪传感器一，20-三轴陀螺仪传感器二，21-三轴陀螺仪传感器三，22-加速度计模块一，23-加速度计模块二，
24-加速度计模块三，25-红外测距传感器，26-触觉传感器一，27-触觉传感器二。
具体实施方式
[0046] 下面结合附图对本发明作进一步说明。
[0047] 图1.1-1.2为具有模仿学习机制的手把手示教机械臂系统的结构图。包括一个底盘、由机械臂连杆、手爪、舵机组成的手臂部分、控制器以及电池，由于机械臂整体惯性较大，为使机械臂系统在运动过程中稳定不倾斜，将控制器和电池固定于底盘连接手臂部分的另一侧以保持平衡。
[0048] 图2为机械臂系统的组成框图。机械臂系统由四部分组成，分别是感知模块、学习模块、电源模块以及执行模块。感知模块包含三轴陀螺仪传感器、加速度计、红外测距传感器、超声波传感器以及触觉传感器，学习模块为DSP控制器，电源模块包含锂电池和电压转化器，执行模块包含51单片机最小系统、舵机控制器和六个舵机。
[0049] DSP控制器安装在机械臂的底盘上，是控制系统的核心。DSP控制器的主处理芯片采用的是TI公司的TMS320F28335芯片，运行速度为150MIPS。该控制器集成8bit-SPI的D/A转换，4路输出；16路AD输入接口；2路UART串行接口，一路为RS232接口；16路PWM输出，可以满足本系统所需的多传感器数据处理和模仿学习算法的运行，以达到机械臂系统的控制要求。
[0050] 51单片机最小系统作为双层结构安装在DSP控制器之上，作为执行模块中输出控制策略的控制器。由STC公司生产的STC89C52单片机构成的最小系统，规格为9cm×7.6cm，该系统集成32个IO口，3个16位定时器，且电源处采用钽电容设计，工作稳定性强，符合作为下位机来控制舵机控制器的要求。
[0051] 舵机控制器作为双层结构安装在DSP控制器之上，作为执行模块中直接控制舵机转动的执行器。32路舵机控制器最多可以同时控制32个舵机，完全满足多自由度机械臂的需求，其中8路带有过载保护，该控制器可以和51单片机进行通信以实现智能控制，且自带PS2通讯接口，可以实现手柄操作，丰富示教形式。
[0052] 舵机作为连接机械臂各个连杆的关节，从底座到手爪共安装六个。舵机采用的是MG996R模拟舵机，尺寸为40.7mm×19.7mm×42.9mm，最大转动角度为180°，工作电压为4.8V-7.2V，扭矩为15kg×cm，适用于机械臂关节舵机。
[0053] 超声波传感器安装在机械臂底座连杆上，用于检测目标物体所在的方向，底座舵机进行180°旋转，当超声波传感器检测到正前方存在目标物体时，舵机停止旋转。所选传感器的探测距离为2cm-450cm，精度可达0.3cm，采用IO出发测距，自动发送8个40kHz的方波；自 动检测是否有信号返回；有信号返回，通过IO输出一高电平，高电平持续的时间就是超声波从发射到返回的时间。最终测试距离S＝高电平时间×声速/2。
[0054] 三个三轴陀螺仪传感器和三个加速度计模块分别安装在机械臂中间三个执行整体动作的连杆上，分别检测三个连杆位姿坐标的角速度和角加速度，但是陀螺仪会产生漂移误差，并会随着时间的推移而累加变大，通过积分会使得误差变得很大，因此不能单独使用陀螺仪作为本系统检测示教过程的传感器，所以配合加速度计模块使用，联合检测机械臂连杆的运动角度。三轴陀螺仪传感器主芯片采用L3G4200D，工作电压为3-5v，测量范围为250/500/2000dps，通信方式采用IIC/SPI通信协议。加速度计模块主芯片采用MPU6050，工作电压为3-6v，尺寸为15.24mm×15.24mm×2mm，测量量程加速度为±16g，角速度为±
2000°/s，通信方式采用IIC/TTL通信协议。
[0055] 红外测距传感器安装在控制手爪方向的舵机上，检测手爪到目标物体的距离。红外测距传感器采用日本SHARP的GP2Y0A02YK0F红外线传感器，模拟信号输出。被测物体的材质、环境温度以及测量时间不会影响传感器的测量精度。传感器输出电压值对应探测距离，测距范围为20-150cm，工作电压4.5v-5.5v。
[0056] 触觉传感器安装在机械臂手爪的夹取处，检测手爪是否夹取到物体。触觉传感器采用MEAS薄膜压电传感器，用于弹性，触觉，振动和冲击的测量。当薄膜来回动作时会产生那一个小的交流大电压(高达±90v的)。
[0057] 电源模块安装在DSP控制器的下方，采用一块输出电压为7.4V的锂电池，容量为2200mah，重量116g，尺寸为105mm×33.5mm×15mm。配置一个电压转换器，将7.4V转换成5V和6.5V。
[0058] 本发明所述机械臂系统运行过程流程图如图3所示。首先对机械臂进行手把手示教，由感知模块采集示教行为信息并进行处理，然后根据目标物体位置确定机械臂初始状态，系统进行模仿学习并再现示教行为动作。所述机械臂系统模仿学习的方法包括以下步骤：
[0059] 步骤1，手把手示教及示教数据处理。
[0060] 此阶段电源模块仅为DSP控制器(2)、三轴陀螺仪传感器一(19)、三轴陀螺仪传感器二(20)、三轴陀螺仪传感器三(21)和加速度计模块一(22)、加速度计模块二(23)、加速度计模块三(24)供电，其它元器件处于断电状态。对机械臂采取手把手离线示教方式，拖拽机械臂夹取目标物体，由感知模块采集示教行为信息。手把手示教及示教数据处理的流程图如图4所示，具体包括以下步骤：
[0061] 步骤1.1，初始化机械臂系统。
[0062] 配置DSP控制器2寄存器状态，初始化全局和局部变量，初始化传感器的位置和舵机的控制量。电源模块仅为DSP控制器(2)、三轴陀螺仪传感器和加速度计模块供电，其它元器件处于断电状态。
[0063] 步骤1.2，对机械臂采用手把手离线示教的方式，拖拽机械臂使机械臂手爪(11)夹住与机械臂系统底盘(1)在同一平面上的目标物体，拖拽时保持手爪对向目标物体。在此过程中，由三轴陀螺仪传感器和加速度计模块组成的动作检测装置采集所在连杆位姿坐标的角速度和角加速度，由红外测距传感器采集到目标物体的距离。为保证示教的质量，使传感器能够精确地采集信号，以较缓慢的速度手把手拖拽机械臂手爪夹取目标物体，全程所用时间控制在5s-10s，并采取3-5次示教行为。
[0064] 步骤1.3，DSP控制器(2)将采集到的位姿坐标角速度和角加速度进行数据处理并通过自适应加权融合算法进行融合，将采集到的距离转化成手爪到目标物体距离，从而得到示教行为状态信息。
[0065] (1)三轴陀螺仪和加速度计的数据处理。
[0066] 陀螺仪用于测量连杆角速度信号,通过对角速度积分,便能得到角度值。每个三轴陀螺仪传感器每1ms采集一个数据，每10个值进行一次数据处理，由于时间很短，忽略时间差异带来的误差，三轴陀螺仪传感器得到的角度计算为：
[0067] angleAn＝angleAn-1+gyron×dt
[0068] 其中angleAn为陀螺仪第n次采样到的角度值，angleAn-1为陀螺仪第n-1次采样到的角度值，gyron为陀螺仪第n次采样到的瞬时角速率值，dt为手把手示教过程所用时间。
[0069] 加速度计用于测量连杆的线性加速度，加速度计的输出值与倾角呈非线性关系，随着倾角的增加而表现为正弦函数变化，同三轴陀螺仪传感器采集数据的方法，加速度计模块得到的角度计算为：
[0070] angleB＝atan2(y,z)×(180/3.14)
[0071] 其中定义加速度计模块x轴朝下，y轴朝前。angleB为加速度计获得的角度，atan2(y,z)为该倾角竖直方向的弧度。
[0072] (2)使用自适应加权融合算法进行数据融合。
[0073] 取三轴陀螺仪传感器采集到的信号angleAn和加速度计模块采集到的信号angleB，采用自适应加权融合算法方法进行数据融合，它无需传感器测量数据的任何先验知识，即可融合出 方差最小的数据融合值，估计后的方差小于单个传感器估计的方差和采用多传感器均值平均做估计的方差。机械臂系统各连杆的真实倾角状态信息sD计算公式如下：
[0074] sD＝W1×angleAn+W2×angleB
[0075] 其中，W1为三轴陀螺仪传感器的最优加权因子，W2为加速度计模块的最优加权因子，故示教行为状态信息sD＝(sD1,sD2,...,sDn)。
[0076] (3)手爪到目标物体距离转化。
[0077] 设红外测距传感器(25)检测到与目标物体距离为d，由于传感器发射口距离手爪夹取中心位置为7cm，故实际手爪到目标物体距离为L＝d-7cm。机械臂系统的示教及示教数据处理的阶段结束。
[0078] 步骤2，机械臂系统确定初始状态。
[0079] 此阶段由系统搭载的超声波传感器(18)和红外测距传感器(25)搜寻目标物体所处的位置，确定机械臂的初始姿态，使系统在改变机械臂初始姿态或目标物体所在方位时，依旧能够完成任务。机械臂系统确定初始状态的流程图如图5所示，具体包括以下步骤：
[0080] 步骤2.1，初始化机械臂系统。
[0081] 配置DSP控制器(2)和51单片机最小系统(3)的寄存器状态，初始化全局和局部变量，初始化传感器的位置以及舵机和舵机控制器(4)的控制量。
[0082] 步骤2.2，机械臂还原到初始姿态。
[0083] 51单片机最小系统(3)通过控制程序，使机械臂还原到手把手示教前的初始姿态，使机械臂在模仿学习示教行为之前，各连杆保持与手把手示教前相同的位姿坐标，以确保即使改变机械臂初始姿态系统仍能完成模仿学习任务。
[0084] 步骤2.3，目标物体方向的确定。
[0085] 51单片机最小系统(3)通过控制程序，使舵机(12)从左向右进行180°旋转，搜寻与系统处于同一桌面的目标物体，当舵机(12)所在连杆上的超声波传感器(18)检测到正前方的目标物体时，舵机(12)停止转动且固定在当前位置，确定了目标物体所在方向，以确保即使改变目标物体所在方位时系统仍能完成模仿学习任务。
[0086] 步骤2.4，手爪与目标物体方向的确定。
[0087] 由于红外测距传感器(25)安装在与机械臂手爪平行位置，红外线发射方向为手爪爪尖方向，因此51单片机最小系统(3)通过控制程序，使舵机(15)向下转动直到红外测距传感器正对向目标物体，即手爪与目标物体处于正对方向，机械臂系统确定初始状态的阶段结 束。
[0088] 步骤3，机械臂系统模仿学习示教行为。
[0089] 此阶段通过模仿学习算法由DSP控制器(2)获得动作策略，51单片机最小系统(3)通过动作策略控制机械臂进行运动，模仿示教行为动作，通过红外测距传感器(25)判定是否完成模仿任务，循环运行，直到红外测距传感器检测到手爪到达目标物体后，机械臂手爪闭合，完成示教行为的模仿。机械臂系统系统模仿学习示教行为的流程图如图6所示，具体包括以下步骤：
[0090] 步骤3.1，初始化机械臂系统。
[0091] 配置DSP控制器(2)和51单片机最小系统(3)的寄存器状态，初始化全局和局部变量，初始化传感器的位置以及舵机和舵机控制器(4)的控制量。
[0092] 步骤3.2，对机械臂系统使用模仿学习算法，采用前馈神经网络的方法获得控制策略，如图7构建一个BP神经网络。以由陀螺仪和加速度计在示教行为中采集的连杆某时刻状态sD＝(sD1,sD2,...,sDn)作为神经网络的输入层，输入层有n个神经元；以h＝(h1,h2,...,hp)为隐含层的输出，输入层与隐含层的连接权值为wj＝(wj1,wj2,...,wjp)，隐含层神经元阈值为bj＝(bj1,bj2,...,bjp)，隐含层传递函数为f1(·)，隐含层有p个神经元；以相应时刻的动作策略a＝(a1,a2,...,aq)作为神经网络的输出层，隐含层与输出层的连接权值为wk＝(wk1,wk2,...,wkq)，输出层神经元阈值为bk＝(bk1,bk2,...,bkq)，输出层传递函数为f2(·)，输出层有q个神经元。
[0093] (1)选取t个输入样本，即t时刻连杆的状态作为输入sD(t)＝(sD1(t),sD2(t),...,sDn(t))，根据BP神经网络原理，采用FR共轭梯度法训练网络，可以得到网络隐含层输出为：
[0094]
[0095] (2)利用输出层各神经元的误差偏导数δ(t)和网络隐含层各神经元的输出h(t)来修正连接权值wk(t)：
[0096]
[0097] 其中wkN+1为更新后的连接权值，wkN为更新前的连接权值，η为学习率。
[0098] (3)最终得到神经网络输出t时刻的动作策略表示为：
[0099]
[0100] 步骤3.3，DSP控制器将训练得到动作策略a(t)传输给单片机最小系统，由单片机控制舵 机控制器使机械臂进行运动，模仿手爪到目标物体的示教行为动作。机械臂执行完动作后，由红外测距传感器检测与目标物体的距离信息并传送给DSP控制器，即检测手爪是否到达目标物体。若红外测距传感器检测到手爪与目标物体距离大于0cm或没有检测到目标物体，说明单片机系统执行的模仿学习动作未能使手爪到达目标物体或偏离目标物体，则此动作策略失败，系统重新回到步骤3.2继续进行模仿学习获得新的动作策略，直到单片机最小系统通过满足设定模仿结束条件的动作策略a'(t)使机械臂手爪到达目标物体位置，即红外传感器检测手爪到目标物体的距离信息L＝0cm时，机械臂停止运动。
[0101] 步骤3.4，此时通过单片机控制舵机(17)开始转动，转动方向为使机械臂手爪闭合方向，当贴在手爪夹取位置的触觉传感器一(26)、触觉传感器二(27)检测到信号时说明手爪已经夹住目标物体，舵机(17)停止转动，机械臂系统实现对目标物体的夹取，至此完成了对全部示教行为的模仿，机械臂系统模仿学习示教行为的阶段结束，从而整个机械臂系统工作结束。
