[发明专利]具有模仿学习机制的手把手示教机械臂系统及方法

权利要求书

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)的上下手爪处。

2.具有模仿学习机制的手把手示教机械臂系统方法，其特征在于：机械臂系统工作时，首先采用手把手示教方法对机械手臂进行示教，由感知模块采集示教信息后经控制系统进行模仿学习，控制机械臂模仿示教行为动作；

(1)手把手示教及示教数据处理

当机械臂系统启动后，系统的舵机控制器不上电，即机械手臂上舵机处于断电状态，采用离线示教的方式，仅感知模块进行工作；用手拖动机械臂完成夹取桌面上物体的动作，三个三轴陀螺仪传感器和加速度计模块组成的动作检测装置采集在示教过程中三个手臂连杆的位姿坐标旋转角速度和角加速度信息，红外测距传感器采集手爪到目标物体的距离信息；动作检测装置将采集到的示教行为信号和距离信号通过IO口传送给DSP控制器，DSP控制器将采集到的信号进行处理后，获得示教行为的状态信息；

(2)机械臂系统确定初始状态

为51单片机最小系统和舵机控制器上电，即机械手臂上舵机处于通电状态；系统首先通过单片机使机械臂还原到手把手示教前所处的姿态，然后系统通过搭载的传感器搜寻确定目标物体的方位，底座舵机搭载着的超声波传感器进行旋转确定目标物体所在方向，手爪方向的舵机进行旋转使其搭载的红外测距传感器与目标物体保持正对方向，机械臂保持此姿态；

(3)机械臂系统模仿学习示教行为

机械臂系统进入模仿学习阶段，构建前馈神经网络，将示教行为的状态信息作为网络的输入，机械臂运动的动作策略作为网络的输出；DSP控制器将获得的动作策略传送给51单片机最小系统，由单片机控制舵机控制器将关节角转化为脉宽信号以控制机械臂上的关节转动相应角度，使手爪向目标物体方向运动；机械臂运动过后由红外测距传感器采集手爪与目标物体的距离信息并传送给DSP控制器，若手爪与目标物体距离大于0cm或检测不到目标物体，则继续进行模仿学习，重新调整动作策略，产生舵机控制信号，循环运行，直至满足设定的模仿结束条件即红外测距传感器检测到手爪与目标物体距离为0cm，手爪闭合，由机械臂手爪处的触觉传感器检测是否夹取到目标物体，从而完成示教行为动作。

3.根据权利要求2所述的具有模仿学习机制的手把手示教机械臂系统方法，其特征在于：首先对机械臂进行手把手示教，由感知模块采集示教行为信息并进行处理，然后根据目标物体位置确定机械臂初始状态，系统进行模仿学习并再现示教行为动作；所述机械臂系统模仿学习的方法包括以下步骤：

步骤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)将采集到的位姿坐标角速度和角加速度进行数据处理并通过自适应加权融合算法进行融合，将采集到的距离转化成手爪到目标物体距离，从而得到示教行为状态信息；

(1)三轴陀螺仪和加速度计的数据处理；

陀螺仪用于测量连杆角速度信号,通过对角速度积分,便能得到角度值；每个三轴陀螺仪传感器每1ms采集一个数据，每10个值进行一次数据处理，由于时间很短，忽略时间差异带来的误差，三轴陀螺仪传感器得到的角度计算为：

angleA_n＝angleA_n-1+gyro_n×dt

其中angleA_n为陀螺仪第n次采样到的角度值，angleA_n-1为陀螺仪第n-1次采样到的角度值，gyro_n为陀螺仪第n次采样到的瞬时角速率值，dt为手把手示教过程所用时间；

加速度计用于测量连杆的线性加速度，加速度计的输出值与倾角呈非线性关系，随着倾角的增加而表现为正弦函数变化，同三轴陀螺仪传感器采集数据的方法，加速度计模块得到的角度计算为：

angleB＝atan2(y,z)×(180/3.14)

其中定义加速度计模块x轴朝下，y轴朝前；angleB为加速度计获得的角度，atan2(y,z)为该倾角竖直方向的弧度；

(2)使用自适应加权融合算法进行数据融合；

取三轴陀螺仪传感器采集到的信号angleA_n和加速度计模块采集到的信号angleB，采用自适应加权融合算法方法进行数据融合，它无需传感器测量数据的任何先验知识，即可融合出方差最小的数据融合值，估计后的方差小于单个传感器估计的方差和采用多传感器均值平均做估计的方差；机械臂系统各连杆的真实倾角状态信息s_D计算公式如下：

s_D＝W₁×angleA_n+W₂×angleB

其中，W₁为三轴陀螺仪传感器的最优加权因子，W₂为加速度计模块的最优加权因子，故示教行为状态信息s_D＝(s_D1,s_D2,...,s_Dn)；

(3)手爪到目标物体距离转化；

设红外测距传感器(25)检测到与目标物体距离为d，由于传感器发射口距离手爪夹取中心位置为7cm，故实际手爪到目标物体距离为L＝d-7cm；机械臂系统的示教及示教数据处理的阶段结束；

步骤2，机械臂系统确定初始状态；

此阶段由系统搭载的超声波传感器(18)和红外测距传感器(25)搜寻目标物体所处的位置，确定机械臂的初始姿态，使系统在改变机械臂初始姿态或目标物体所在方位时，依旧能够完成任务；

具体包括以下步骤：

步骤2.1，初始化机械臂系统；

配置DSP控制器(2)和51单片机最小系统(3)的寄存器状态，初始化全局和局部变量，初始化传感器的位置以及舵机和舵机控制器4的控制量；

步骤2.2，机械臂还原到初始姿态；

51单片机最小系统(3)通过控制程序，使机械臂还原到手把手示教前的初始姿态，使机械臂在模仿学习示教行为之前，各连杆保持与手把手示教前相同的位姿坐标，以确保即使改变机械臂初始姿态系统仍能完成模仿学习任务；

步骤2.3，目标物体方向的确定；

51单片机最小系统(3)通过控制程序，使舵机(12)从左向右进行180°旋转，搜寻与系统处于同一桌面的目标物体，当舵机(12)所在连杆上的超声波传感器(18)检测到正前方的目标物体时，舵机(12)停止转动且固定在当前位置，确定了目标物体所在方向，以确保即使改变目标物体所在方位时系统仍能完成模仿学习任务；

步骤2.4，手爪与目标物体方向的确定；

由于红外测距传感器(25)安装在与机械臂手爪平行位置，红外线发射方向为手爪爪尖方向，因此51单片机最小系统(3)通过控制程序，使舵机(15)向下转动直到红外测距传感器正对向目标物体，即手爪与目标物体处于正对方向，机械臂系统确定初始状态的阶段结束；

步骤3，机械臂系统模仿学习示教行为；

此阶段通过模仿学习算法由DSP控制器(2)获得动作策略，51单片机最小系统(3)通过动作策略控制机械臂进行运动，模仿示教行为动作，通过红外测距传感器(25)判定是否完成模仿任务，循环运行，直到红外测距传感器检测到手爪到达目标物体后，机械臂手爪闭合，完成示教行为的模仿；具体包括以下步骤：

步骤3.1，初始化机械臂系统；

配置DSP控制器(2)和51单片机最小系统(3)的寄存器状态，初始化全局和局部变量，初始化传感器的位置以及舵机和舵机控制器(4)的控制量；

步骤3.2，对机械臂系统使用模仿学习算法，采用前馈神经网络的方法获得控制策略，以由陀螺仪和加速度计在示教行为中采集的连杆某时刻状态s_D＝(s_D1,s_D2,...,s_Dn)作为神经网络的输入层，输入层有n个神经元；以h＝(h₁,h₂,...,h_p)为隐含层的输出，输入层与隐含层的连接权值为w_j＝(w_j1,w_j2,...,w_jp)，隐含层神经元阈值为b_j＝(b_j1,b_j2,...,b_jp)，隐含层传递函数为f₁(·)，隐含层有p个神经元；以相应时刻的动作策略a＝(a₁,a₂,...,a_q)作为神经网络的输出层，隐含层与输出层的连接权值为w_k＝(w_k1,w_k2,...,w_kq)，输出层神经元阈值为b_k＝(b_k1,b_k2,...,b_kq)，输出层传递函数为f₂(·)，输出层有q个神经元；

(1)选取t个输入样本，即t时刻连杆的状态作为输入s_D(t)＝(s_D1(t),s_D2(t),...,s_Dn(t))，根据BP神经网络原理，采用FR共轭梯度法训练网络，可以得到网络隐含层输出为：

h(t)=f1(Σi=1nwjpsi(t)-bjp)]]>

(2)利用输出层各神经元的误差偏导数δ(t)和网络隐含层各神经元的输出h(t)来修正连接权值w_k(t)：

wkN+1=wkN+ηδ(t)h(t)]]>

其中w_k^N+1为更新后的连接权值，w_k^N为更新前的连接权值，η为学习率；

(3)最终得到神经网络输出t时刻的动作策略表示为：

a(t)=f2(Σh=1pwkqhh(t)-bkq)]]>

步骤3.3，DSP控制器将训练得到动作策略a(t)传输给单片机最小系统，由单片机控制舵机控制器使机械臂进行运动，模仿手爪到目标物体的示教行为动作；机械臂执行完动作后，由红外测距传感器检测与目标物体的距离信息并传送给DSP控制器，即检测手爪是否到达目标物体；若红外测距传感器检测到手爪与目标物体距离大于0cm或没有检测到目标物体，说明单片机系统执行的模仿学习动作未能使手爪到达目标物体或偏离目标物体，则此动作策略失败，系统重新回到步骤3.2继续进行模仿学习获得新的动作策略，直到单片机最小系统通过满足设定模仿结束条件的动作策略a'(t)使机械臂手爪到达目标物体位置，即红外传感器检测手爪到目标物体的距离信息L＝0cm时，机械臂停止运动；

步骤3.4，此时通过单片机控制舵机17开始转动，转动方向为使机械臂手爪闭合方向，当贴在手爪夹取位置的触觉传感器一(26)、触觉传感器二(27)检测到信号时说明手爪已经夹住目标物体，舵机(17)停止转动，机械臂系统实现对目标物体的夹取，至此完成了对全部示教行为的模仿，机械臂系统模仿学习示教行为的阶段结束，从而整个机械臂系统工作结束。