1.一种基于iPhone智能手机的机器人人机交互系统，其特征在于，该系统可利用智能手机这类通用设备进行具备人体动作解析功能的高效便捷的机器人遥操作，并构建在控制器局域网络CAN总线和INTERNET因特网络上的分布开放式的可重构人机交互系统，该系统包括：

iPhone控制端（1）：采用智能手机这类人们日常生活随身携带的通用设备，通过Internet（7）与服务器端（2）和网络视频模块（6）相连接，利用iPhone成熟的图形界面技术为人机交互提供完善的界面，用于接收用户的触屏输入及人体动作控制输入，并将输入的相应控制操作信息编码后经由Internet（7）发送至服务器端（2），由服务器端（2）进行指令解析，同时显示由网络视频模块（6）得到的反馈视频；

服务器端（2）：通过CAN总线（3）与控制器端（4）、机器人端（5）相连，用于通过Internet（7）接收由iPhone控制端（1）发送的控制操作信息，对于人体动作信息利用隐马尔科夫模型HMM进行解析，各种控制操作信息最终解析为一系列笛卡尔坐标系下控制指令，这些指令由服务器端（2）通过CAN总线（3）发给控制器端（4）；服务器端（2）向控制器端（4）发送指令信息的同时亦从CAN总线（3）上捕获机器人端（5）的状态信息，机器人端（5）的实际指令控制仅由控制器端（4）实现；

控制器端（4）：控制器端（4）负责完成机器人底层的运动控制，包括正、逆运动学，微分运动控制，离线和在线轨迹规划，连续轨迹规划；通过CAN总线（3）与机器人端（5）相连，用于接收由服务器端（2）发送的笛卡尔坐标下的控制指令；控制器端（4）首先利用逆运动学计算出目标位置的机器人端（5）可实现的关节角度值，再进行轨迹规划来对各时刻机器人端（5）的位姿进行精确控制，从而完成某一基本指定动作；

机器人端（5）：这里采用RH6机器人实际执行控制动作，完成交互需求；由于采用开放通用的可重构式体系结构，本系统并不仅仅实现对RH6机器人的控制，同时可扩展应用于各种家用服务性机器人、普通专用机器人设备；

网络视频模块（6）：系统中网络视频模块使用网络摄像头来得到反馈图像，用于使用户直观地得知当前机器人的状态位姿，并可由iPhone控制端调整摄像头姿态。

2.如权利要求1所述的基于iPhone智能手机的机器人人机交互系统，其特征在于，选择iPhone这类通用便携设备作为交互系统的控制端，并充分利用iPhone控制端（1）的MULTI-TOUCH触屏与其内置3维加速计，实现了具有基于触屏的手势识别及人体动作解析功能的多通道人机交互；同时借助iPhoneOS开发平台成熟的图形界面技术及socket编程完成具备网络视频反馈功能的良好用户体验网络机器人交互软件；最终实现无需额外设备便可随时随地进行高效自然、灵活友好的多模态人机交互。

3.如权利要求1所述的基于iPhone智能手机的机器人人机交互系统，其特征在于，该人机交互系统采用了开放分布式可重构系统结构，将控制交互设备、控制指令解析、机器人运动控制划分为各个分离无关模块，各模块通过Internet（7）及CAN总线（3）进行通讯交互，并利用windos、linux这里通用平台及C++程序语言进行开发，模块间留有独立松散的接口，可根据具体需求对相关模块进行扩展或裁减，具有传统机器人控制系统不具备的较强的可重构性。

技术领域

[0001]本发明涉及机器人控制技术领域，特别是涉及一种基于通用便携、控制灵活、无距离限制的高友好度的iPhone智能手机的机器人人机交互系统。

背景技术

[0002]21世纪，服务型机器人在世界各地都受到了极大的推崇。国内外均将发展服务型机器人列为重点项目课题，或已开始投入使用。中国工程院院士徐扬生说过，“人类的一只脚已经迈入了机器人时代的大门，未来10年，机器人将融入中国百姓的日常生活”。虽然服务型机器人有着广阔的发展前景，但是机器人自诞生以来，所采用的控制器基本都是开发者基于自己的封闭独立系统架构开发的。它们采用专用计算机、专用机器人编程语言、专用操作系统，其昂贵的费用及专用性将严重制约服务型机器人走进普通家庭这一长远目标。另一方面，这些专用的设备不仅不便携带，而且在使用范围上受到限制，从用户角度考虑，现有的控制手段（如鼠标、键盘、终端显示等IO设备）也不够人性化。一种通用的，廉价的，使用方便的服务型机器人控制器的研发势在必行。

[0003]为构建具有良好用户体验的机器人人机交互系统，应选择以人为中心的交互方式及相应设备。纵观人机交互的发展，其经历了命令界面、图形界面、多媒体界面等主要阶段，目前正向虚拟现实技术和多通道用户界面的方向发展。早期使用终端显示与键盘等设备基于字符界面实现人机交互。这类交互方式对于某些问题（计算密集型问题）或是应用情境（嵌入式系统开发）是简洁高效的，但其抽象复杂性及对记忆负荷要求令非专业人员望而生畏，且不适于处理空间的、非数值的问题（图像文字处理）。当前主流的人机交互方式采用户图形界面（WINP/GUI）或多媒体用户界面，也是时下国内外机器人控制系统普遍采用的交互方式，主要使用键盘、鼠标与显示器等IO设备。与传统命令界面相比，具有图形界面的人机交互更简便直观，尤其是多媒体技术引入了音频、视频等动态媒体，大大丰富了计算机表现信息的形式。然而，由于这类交互方式仍使用键盘、鼠标等常规输入设备，这种单通道的输入不仅存在人机交互通信带宽不平衡的瓶颈，而且效率差、缺乏灵活性，不符合人们自然行为习惯。多通道用户界面与虚拟现实技术这类多模态人机交互方式可以弥补图形界面方式人机交互的不足，利用摄像头、触屏、三维鼠标、力矩球等设备并通过表情解释、OCR与手写识别、手势解释等技术可以实现具有极好交互性的自然、灵活的人机交互。目前在机器人控制领域，尤其是服务机器人，也有基于此类技术的相关研究，如语音控制和手势识别控制等。其中应用的这些交互技术，如语音控制技术，虽然控制直接方便，但有易受操作环境噪音干扰、识别精度低、能识别的输入信息有限等缺点，技术尚待成熟；又如虚拟现实技术与手势识别技术，需要佩戴传感器或是需要仿真头盔、摄像投影仪等额外设备，难以随身携带随时使用。不仅如此，由于控制机器人使用了专用控制器，只能对既定类型机器人进行控制，其封闭专用的体系结构也难以进行扩展及改进。

发明内容

[0004]本发明目的是解决现有的机器人控制人机交互系统使用不便、封闭专用等的不足，提供一种基于iPhone智能手机的机器人人机交互系统。利用智能手机这类通用设备来构建上述便携通用的人机交互系统，无需额外设备便可随时随地的实现便捷高效的远程机器人遥操作。

[0005]本发明提供的基于iPhone智能手机的机器人人机交互系统包括：

[0006]iPhone控制端（1），通过Internet（7）与服务器端（2）和网络视频模块（6）相连接，利用iPhone成熟的图形界面技术为人机交互提供完善的界面，用于接收用户的触屏输入及人体动作控制输入，并将输入的相应控制操作信息编码后经由Internet（7）发送至服务器端（2），由服务器端（2）进行指令解析；利用iPhoneSDK提供的webview控件以及NAT静态端口映射技术显示网络视频模块的视频反馈；所述iPhone控制端（1）为iPhone手机这类通用设备，向用户提供高效自然、灵活友好的多模态人机交互；不仅具备人体动作解析功能，且通用便携，无需额外设备便可随时随地利用3G功能通过Internet（7）对机器人进行突破距离限制的遥操作。

[0007]服务器端（2），通过CAN总线（3）与控制器端（4）和机器人端（5）相连，用于通过Internet（7）接收由iPhone控制端（1）发送的控制操作信息，对于人体动作信息利用HMM进行解析，各种控制操作信息最终解析为一系列笛卡尔坐标系下控制指令，这些指令由服务器端（2）通过CAN总线（3）发给控制器端（4）；服务器端（2）向控制器端（4）发送指令信息的同时亦从CAN总线（3）上捕获机器人端（5）的状态信息，机器人端（5）的实际指令控制仅由控制器端（4）实现；

[0008]CAN总线（3）分别与服务器端（2）、控制器端（4）和机器人端（5）相连；

[0009]控制器局域网络CAN总线（3），是一种通用工业总线，用于形成一个CAN总线网络控制环境；

[0010]控制器端（4）：其上运行的是QNX实时操作系统，控制周期是16ms；通过CAN总线（3）与机器人端（5）相连，用于接收由服务器端（2）发送的笛卡尔坐标下的控制指令；控制器端（4）首先利用逆运动学计算出目标位置的机器人端（5）可实现的关节角度值，再进行轨迹规划来对各时刻机器人端（5）的位姿进行精确控制，从而完成某一基本指定动作；

[0011]机器人端（5）：本系统中采用RH-6双机械臂机器人作为机器人端，用于实际执行控制动作，完成交互需求；由于采用开放通用的可重构式体系结构，本系统并不仅仅实现对RH6机器人的控制，同时可扩展应用于各种家用服务性机器人、普通专用机器人等设备。

[0012]网络视频模块（6）：系统中网络视频模块使用具备调整视角、变焦等功能的网络摄像头来得到反馈图像，将当前机器人端（5）的光学信息转为反馈视频经由Internet（7）发至iPhone控制端（1）显示，使用户直观地得知当前机器人的状态位姿，并可由iPhone控制端调整摄像头姿态。

[0013]此外，由于各模块间留有通用接口，对于多用户、多机器人的应用需求，可进一步扩展服务器端的功能，实现共享的机器人资源管理。

[0014]本项目设计并实现了基于iPhone智能手机的网络机器人人机交互系统，采用开放可重构的分布式体系结构，只需通过手机这类通用设备便可对网络机器人进行便捷灵活的空间6自由度控制，其高友好度的人机接口具有新颖的人体动作解析、MULTI-TOUCH手势解析及视频反馈等功能。本系统进行了如图3所示的实际网络环境的往返时间（RTT）测试。在50次独立网络环境测试中平均RTT为86毫秒，远小于人的反应时间。实验结果充分表明，此系统具有良好的实时性与人机交互性能。将机器人控制融入日常生活的技术实现，随着服务性机器人进入千家万户，将极大地改变人们未来的生活模式。

[0015]本发明的优点和积极效果：

[0016]本发明突破传统机器人控制的复杂笨重及封闭等特点，创新地将iPhone各种硬件资源和先进的交互技术（如MULTI-TOUCH、加速计等）与机器人技术进行有机融合，实现具有基于HMM新颖的人体动作控制解析功能的高易用性人机接口（HMI）。同时，使用最新的3G技术接入Internet来构建具有实时网络视频反馈的远程机器人控制平台，突破控制距离限制，大大提高机器人控制便利性并拓宽应用范围，而且其开放可重构的分布式体系结构也便于系统扩展改进。最终实现一种无需额外设备便可随时随地进行便捷高效的机器人遥操作的人机交互系统。本发明不仅是一种创新实用的机器人控制模式，随着服务性机器人的普及，更将给人们的未来生活带来极大便利。

附图说明

[0017]图1为本发明的人机交互系统体系结构图；

[0018]图2 为HMM状态图；

[0019]图中，1为iPhone控制端，2为服务器端，3为CAN总线，4为控制器端，5为机器人端，6为网络视频模块，7为Internet。

[0020]图3 为本系统实际网络环境的RTT测试结果；

[0021]图中纵轴为RTT，以毫秒为单位。横轴表示50次不同的独立测试。

具体实施方式

[0022]为了使本技术领域的人员更好地理解本发明方案，下面结合附图和实施方式对本发明作进一步的详细说明。

[0023]本人机交互系统采用开放式系统结构，将控制交互设备、控制指令解析、机器人运动控制等划分为各个分离无关模块。各模块利用通用平台（Linux、Windows）及程序语言（C、C++）进行开发，模块间留有独立松散的接口，可根据具体需求对相关部分进行扩展或裁减。

[0024]如图1，本人机交互系统主要包括iPhone控制端（1）、服务器端（2）、控制器端（4）、机器人端（5）及图像采集模块（6）。控制信息从iPhone控制端（1）通过Internet（7）发至服务器端（2）解析为相应操作，由控制器端（4）控制机器人端（5）实现各种动作，而图像采集模块主要生成反馈视频通过Internet（7）发往iPhone控制端（1）显示。下文将对各模块进行进一步说明。

[0025]iPhone控制端（1）通过多线程的socket技术利用3G网络与服务器端进行TCP通讯，基于加速计和MULTI-TOUCH触屏等硬件平台，将用户的动作、触屏手势等信息编码为指定的格式及标记位发送到服务器端解析并接收服务器反馈的检测到的错误信息，同时利用iPhoneSDK提供的webview控件以及NAT静态端口映射技术显示网络视频模块的视频反馈。这里使用iPhone这类人们日常生活随身携带的通用设备进行控制，突破传统机器人控制器专用、笨重的缺陷，无需携带其它设备便可利用手机等设备无线接入Internet，实现对机器人方便快捷且直观的远程控制。

[0026]服务器端（2），服务器端程序使用通用开放的WindowsOS下的标准C++语言开发，利于体系移植重构。服务器端通过Internet接收iPhone控制端发送的用户操作信息并进行解析，然后结合指令标记位将一系列指令码映射为笛卡尔坐标下机器人的某一动作或行为。对于不同应用，服务器端只需将用户的动作操作信息映射为合适的指令即可，从而使系统具备更强的通用可重构性。交互系统中提供了基于行为的机器人控制，提了高机器人部分的自主性，使得控制更智能化，人机交互更加便捷高效并减小遥操作网络时延对控制影响。在本系统中，服务器端、控制器端、机器人端均与同一条CAN总线相连，彼此可通过CAN总线直接通讯。服务器即是通过CAN总线将转化后的笛卡尔空间机器人动作指令发送给控制器端。此外，对于多用户、多机器人的应用需求，可进一步扩展服务器端的功能，实现共享的机器人资源管理。

[0027]现具体说明基于HMM的人机交互算法。

[0028]iPhone的硬件加速计可提供笛卡尔空间下三轴加速值，程序中通过调用accelerometer:didAccelerate方法以60Hz的频率对硬件进行采样，可得到X、Y、Z三轴对应的3个浮点小数，并以重力加速度                                                为单位。一般情况下认为用户手持iPhone设备时某一静态动作的加速值可满足。考虑到用户动作较难产生过大的加速度，这里只将满足此不等式的加速值作为用户的合法动作进行解析。

[0029]iPhone控制端向服务器端发送3字节的三方向加速值及2字节的标记位，具体动作解析在服务器端完成。对于3字节的加速值字段，这里将某一轴向的加速值前三位数编码到一个字节内，避免了加速计末位数值误影响控制质量。第一比特位为符号位，可表示+127到-128范围的值（+1.27g~-1.28g）。考虑到人们的常规动作难以产生较大加速值，故丢弃超出范围的采样值。对于2字节的状态标记字段，其中包括控制指令类型（如特殊指令、触屏指令、动作控制指令等）与响应模式、指定的控制参数等信息。对于人体动作控制，指令类型即标记为动作控制指令。在具体实现中，我们将加速值序列换算为用户手持iPhone的实际倾斜动作方向与角度，并基于隐马尔可夫模型（HMM）映射为机器人动作的类别与幅度。其中iPhone设备的倾斜角度与对应机器人动作幅度正相关。考虑到硬件加速计误差影响，我们将iPhone设备的姿态空间按角度值适当划分为若干离散区域，以免控制机器人运动时出现抖动。为提高交互质量，使控制更直观便捷，iPhone设备各方向的倾斜控制具备叠加效果，如设备左前倾斜则机器人进行前倾与左倾对应动作的复合运动。

[0030]为将一段加速值序列解析为用户的倾斜操作动作，这里利用HMM来解决这一问题。HMM是一个双重随机过程，用户的操作动作可视为其基本随机过程，由人体动作产生的加速值序列则是我们可观测到的以概率分布与基本状态相联系的事件。如图2所示即为基于HMM解析用户意图的状态示意图。对于给定的加速值序列及模型参数，我们需要寻找最可能的能产生此特定输出序列的隐含状态（用户操作动作）。显然HMM模型能较好地描述人体动作控制这一随机模式。对于一个HMM{}，本问题将模型中各参数定义如下：

[0031]（1）隐含状态集为用户各种可能的操作动作，如停止、向前慢速平动等。其中N为隐含状态的数目，且为停止状态，t时刻所处的状态为。考虑到复合控制，隐含状态数为129种，即主要、次要倾斜方向及相应倾斜程度加上一个停止状态。

[0032]（2）可观测状态为得到的各加速值三元组，其中M为可观测状态数。程序实现中取6组加速值作为观测序列，即M=6。

[0033]（3）模型中为隐含状态转移概率矩阵，且，其中且。这里将其初始值置为

[0034]

[0035]（4）模型中为观测状态转移概率矩阵，且，这里将其初始值置为，其中。

[0036]（5）模型中为初始状态概率矩阵，且，其中。设定。

[0037]初始模型确定以后，利用Baum-Welch算法对初始的HMM模型进行重新计算从而得到新的参数。然后使用Viterbi算法找出当前加速值序列下概率最大的用户操作动作。

[0038]对加速值解析后得到用户动作倾斜类别及幅度，需结合2字节状态标记信息最终确定用户的意图，即应控制机器人具体作出怎样的动作。在动作指令中具有选定轴控制及选定面两种响应模式。前者在标记字段中指定x、y或z轴，完成机器人末端在指定轴向的平动及转动，从而完成了对空间6自由度机器人12种基本动作的控制。后者在标记字段中指定X-Y、X-Z或Y-Z平面，完成机器人末端在指定平面内的平动，控制更加直观便捷。此外，利用HMM的学习功能在iPhone端控制程序中用户可自定义自己偏好的控制方式。

[0039]控制器端（4）：其上运行的是QNX实时操作系统，控制周期是16ms；通过CAN总线（3）与机器人端（5）相连，用于接收由服务器端（2）发送的笛卡尔坐标下的控制指令；控制器端控（5）由运动控制指令可知机器人末端要达到的控制位置或速度，制器端（4）首先利用逆运动学计算出目标位置的机器人端（5）可实现的关节角度值，再进行轨迹规划来对各时刻机器人端（5）的位姿进行精确控制，从而完成某一基本指定动作。下面对逆运动学、微分控制和轨迹规划的实现方法进行具体说明（但此部分不为本系统重点）。

[0040]本系统采用的RH6机器人各杆件坐标参数如下表：

[0041]

[0042]由此易得相邻关节坐标之间的变换矩阵，且其最终的变换矩阵如下

[0043]

[0044]

[0045]

[0046]

[0047]

[0048]式中

[0049]控制器端（5）从服务器端（2）控制指令得知目标位置。求解逆运动学各关节方法如下：

[0050]（1）求关节1的解：

[0051]由得到：。等式左右各为一个

[0052]4×4的矩阵，分别记做L，R。利用矩阵对应元素相等，可以得到：

[0053]L(2,4)=R(2,4)—→c1*py-px*s1-a6*(ay*c1-ax*s1)=0—→

[0054]tan(t1)=s1/c1=(py-a6*ay)/(px-a6*ax)—→t1=arctan((py-a6*ay)/(px-a6*ax))

[0055]通过机器人工作位置的左右臂关系参数，在解空间（考虑关节限位）中选取一个解，就完成了关节1的求解过程。

[0056]求关节2、3的解：

[0057]（2）：求解2轴的解

[0058]      由L(1,4)=R(1,4)和L(3,4)=R(3,4)

[0059]c1*px+py*s1-a6*(ax*c1+ay*s1)=a1-a2*s2-a3*s23+a4*c23

[0060]pz-a6*az=a2*c2+a4*s23+a3*c23

[0061]—→

[0062]令k1=c1*px+s1*py-a6*(ax*c1+ay*s1)-a1

[0063]令k2=pz-a6*az

[0064]      k1=-a2*s2-a3*s23+a4*c23

[0065]      k2=a2*c2+a4*s23+a3*c23

[0066]      —→

[0067]      k1+a2*s2=-a3*s23+a4*c23

[0068]      k2-a2*c2=a4*s23+a3*c23

[0069]             等式左边平方和等于等式右边平方和

[0070]      —→

[0071]k1*k1+2*a2*s2*k1+a2*a2*s2*s2+k2*k2-2*a2*c2*k2+a2*a2*c2*c2=a3*a3+a4*a4

[0072]—→

[0073]k1*k1+2*a2*s2*k1+k2*k2-2*a2*c2*k2+a2*a2=a3*a3+a4*a4

[0074]2*a2*(k1*s2-k2*c2)+k1*k1+k2*k2+a2*a2-a3*a3-a4*a4=0

[0075]令k3=k1*k1+k2*k2+a2*a2-a3*a3-a4*a4

[0076]—→

[0077]2*a2*(k1*s2-k2*c2)+k3=0

[0078]k1*s2-k2*c2=-k3/(2*a2)

[0079]令k4=-k3/(2*a2)

[0080]k1*s2-k2*c2=k4

[0081]—→

[0082]令k5=sqrt(k1*k1+k2*k2)

[0083]令k6=(abs(k2)/k2)*(k4/k5)

[0084]      t2=acos(k6)-atan(k1/k2)或t2=-acos(k6)-atan(k1/k2)

[0085]（3）：求解三轴的解

[0086]由L(1,4)=R(1,4)和L(3,4)=R(3,4)

[0087]c1*px+py*s1-a6*(ax*c1+ay*s1)=a1-a2*s2-a3*s23+a4*c23

[0088]pz-a6*az=a2*c2+a4*s23+a3*c23

[0089]—→

[0090]令k1=c1*px+s1*py-a6*(ax*c1+ay*s1)-a1

[0091]令k2=pz-a6*az

[0092]—→

[0093]k1+a2*s2=-a3*s23+a4*c23

[0094]k2-a2*c2=a4*s23+a3*c23

[0095]—→

[0096]k1+a2*s2=-a3*(s2*c3+c2*s3)+a4*(c2*c3-s2*s3)

[0097]k2-a2*c2=a4*(s2*c3+c2*s3)+a3*(c2*c3-s2*s3)

[0098]—→

[0099]k1+a2*s2=(-a3*s2+a4*c2)*c3+(-a3*c2-a4*s2)*s3

[0100]k2-a2*c2=(a4*s2+a3*c2)*c3+(a4*c2-a3*s2)*s3

[0101]—→

[0102]令m1=k1+a2*s2

[0103]令m2=k2-a2*c2

[0104]令m3=-a3*s2+a4*c2

[0105]令m4=-a3*c2-a4*s2

[0106]令m5=a4*s2+a3*c2

[0107]令m6=a4*c2-a3*s2

[0108]—→

[0109]m1=m3*c3+m4*s3

[0110]m2=m5*c3+m6*s3

[0111]—→

[0112]s3=(m2*m3-m1*m5)/(m3*m6-m4*m5)

[0113]c3=(m2*m4-m1*m6)/(m3*m6-m4*m5)

[0114]t3=arctan(s3/c3)

[0115]由于机械结构的原因，二轴与三轴要在保证各自关节限位的条件下共同进行选解，然后分别选择最优解。

[0116]（4）关节5求解：

[0117]由L(1,1)=R(1,1)andL(2,1)=R(2,1)andL(3,1)=R(3,1)

[0118]c5*(c2*c3-s2*s3)-c4*(c3*s2+c2*s3)*s5=ax*c1+ay*s1

[0119]-s4*s5=ay*c1-ax*s1

[0120]c5*(c3*s2+c2*s3)-c4*(-c2*c3+s2*s3)*s5=az

[0121]—→

[0122]令n1=ax*c1+ay*s1

[0123]—→

[0124]c5*c23-s23*c4*s5=n1

[0125]c5*s23+c23*c4*s5=az

[0126]-s4*s5=ay*c1-ax*s1

[0127]—→

[0128]c5=az*s23+n1*c23

[0129]t5=arcos(c5)

[0130](5)  关节4求解：

[0131]由上面结果：

[0132]c4*s5=az*c23-n1*s23

[0133]c4=(az*c23-n1*s23)/s5(s5!=0)

[0134]t4=arcos(c4)

[0135](6)关节6求解：

[0136]由L(3,2)=R(3,2)andL(3,3)=R(3,3)

[0137]c4*c5*(-c2*c3+s2*s3)+(c3*s2+c2*s3)*s5=c6*oz+nz*s6

[0138]-(-c2*c3+s2*s3)*s4=-c6*nz+oz*s6

[0139]—→

[0140]-c4*c5*c23+s23*s5=oz*c6+nz*s6

[0141]c23*s4=-nz*c6+oz*s6

[0142]—→

[0143]setn2=-c4*c5*c23+s23*s5

[0144]setn3=c23*s4

[0145]—→

[0146]oz*c6+nz*s6=n2

[0147]-nz*c6+oz*s6=n3

[0148]—→

[0149]setn4=nz*nz+oz*oz

[0150]c6=(n2*oz-n3*nz)/n4when(n4!=0)

[0151]s6=(n2*nz+n3*oz)/n4

[0152]      t6=arctan(s6/c6)

[0153]当已知机器人末端执行器的笛卡儿速度时，需要通过逆雅可比矩阵求出关节的角速度。

[0154]

[0155]其中，为逆雅克比矩阵，为末端的笛卡儿速度（包括线速度和角速度），为机器人关节速度。一般，逆雅克比矩阵的求解方法有两种：一种是求出符号形式的雅克比矩阵的逆，然后把数值代入并计算出速度，另一种是将数据代入雅克比矩阵，然后用高斯消去法或其他类似的方法来求该数值矩阵的逆。由于两种方法的计算量大而且费时，故采用一种替代的方法，用机器人的逆运动学方程来计算关节的速度。

[0156]微分运动控中用于求解逆雅克比选取的六个表达式为：

[0157]

[0158]式中，对其进一步微分处理便可得到逆雅克比矩阵的基本表达式，

[0159]由式(1)求导可得：

[0160]

[0161]由式(2)(3)求导后联立可得：

[0162]

[0163]其中：

[0164]

[0165]再求

[0166]

[0167]由式(4)(5)求导联立可得：

[0168]

[0169]再求

[0170]

[0171]由式(6)求导可得

[0172]

[0173]其中：为各关节速度，为末端的线速度和角速度。

[0174]笛卡尔空间下的运动指令可换算出关节值，但直接利用关节数据可能造成机器人的运动不平滑，造成对机器人关节电机的伤害，所以需用轨迹规划对机器人的运动指令进行规划。这里采用拟合正弦曲线的轨迹规划方法，其细节不必赘述。

[0175]机器人端（5）本系统使用RH6机器人来执行各种控制动作。由于采用开放通用的可重构式体系结构，本系统并不仅仅实现对RH6机器人的控制，同时可扩展应用于各种家用服务性机器人、普通专用机器人等设备。

[0176]网络视频模块（6）系统中网络视频模块使用网络摄像头来得到反馈图像，使用户直观地得知当前机器人的状态位姿，并可由iPhone控制端调整摄像头姿态。考虑到通用兼容性，这里将光学信息转为各平台广泛支持的JPEG格式，通过传输一系列JPEG图像来实现连贯的视频反馈。

[0177]以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。