教学用多自由度机械手末端位姿测量方法与流程

本发明涉及机械手运动控制技术，具体为一种教学用多自由度机械手末端位姿测量方法。

背景技术：

舵机驱动的多自由度机械手是一种低成本的机械装置，因具有结构简单、轻便小巧的特点而受到机器人课堂实践教学的青睐。

舵机驱动的机械手作为一种教学演示器具，受限于成本、体积以及便携性的要求，其机械手指末端通常没有安装位姿测量传感器，如：多目ccd传感器、惯性单元等，因此这种机械手的运动控制理论属开环控制。

教学人员在示教演示机器人运动学、逆运动学相关的教学知识点或学生在开展机械手运动控制实践时，机械手指末端的位姿主要是通过机械手关节的几何参数和运动参数推导而出，但这类方法容易受到机械手关节间隙和传动臂变形等的影响，并且机械手关节数越多，其造成误差就越大，很难真正确定机械手指末端的实际位姿。

从实验可验证性的原则来看，这样的结果会对机器人课堂教学效果产生一定的影响。

技术实现要素：

针对现有技术的不足，本发明提出了一种教学用多自由度机械手末端位姿测量方法。

能够解决现有技术不足的教学用多自由度机械手末端位姿测量方法，采用坐标刻度纸和三只激光笔作为辅助工具测量多关节机械手的机械手指末端位姿，其测量方式为：

1、建立多关节机械手的简化模型、绝对坐标系和相对坐标系，所述简化模型基于坐标刻度纸零点设置，所述坐标刻度纸零点为绝对坐标系的原点，多关节机械手的机械手指末端设为相对坐标系的原点。

2、三只斜向下方的激光笔固定在相对坐标系的原点即机械手指末端，两两激光笔之间的夹角通过测量得出。

3、三只激光笔发出的激光照射到坐标刻度纸上，测出三个照射点于绝对坐标系下的坐标值，利用这3个坐标值和激光笔之间的夹角，运用正弦定理可以建立三个关于机械手指末端坐标点的约束方程，求解出机械手指末端的坐标值，这个坐标值为机械手指末端在绝对坐标系下的位置点，也是机械手在运动控制编程中最重要的输出指令，作为反馈信号输入控制系统。

4、在绝对坐标系和相对坐标系之间建立一个用三个欧拉角来表示的旋转变换矩阵，该旋转变换矩阵用来表示多关节机械手在运动过程中每个时刻下机械手指末端的姿态，这个姿态作为重要的运动编程输入指令，用于控制多关节机械手按照需要完成的任务去运动，作为反馈机制中的信号输入给控制部分。

激光笔之间的角度可根据坐标刻度纸的尺寸大小进行调节，确保激光照射在坐标刻度纸上。

常规上，将两两激光笔之间的角度均设置为相等。

所述多关节机械手的一种结构包括设于坐标刻度纸零点的竖直臂，所述竖直臂上设置第一转动臂，所述第一转动臂上设有第二转动臂，第二转动臂上设有第三转动臂，所述第三转动臂设有转动的机械手指。

本发明的有益效果：

1、本发明教学用多自由度机械手末端位姿测量方法在机械手指末端安装三个激光笔，三个激光笔之间的夹角可以根据底座刻度纸的大小调节，使激光笔发射的激光能完全投影到坐标刻度纸上，投影点的坐标可在坐标刻度纸上直接读出。

2、本发明提供了一种逆向求解机械手指末端位姿的方法，此种逆向求解方法相比较于直接通过机械手关节逐步推导求解机械手指末端位姿过程要简单很多，且对测量方法所要求的机械结构和器材简单。

3、本发明给教学实验带来了极大便利，操作简单易懂。

附图说明

图1为本发明一种实施方式的坐标建模示意图。

图2为图1中三条激光线与坐标刻度纸形成的四面体在相对坐标系下的坐标示意图。

图3为图1中多关节机械手的结构示意图。

图号标识：1、坐标刻度纸；2、多关节机械手；3、竖直臂；4、第一转动臂；5、第二转动臂；6、第三转动臂；7、机械手指；8、底板；9、转台；10、第一舵机；11、第二舵机；12、第三舵机。

具体实施方式

下面结合附图所示实施方式对本发明的技术方案作进一步说明。

本发明教学用多自由度机械手末端位姿测量方法，是采用了坐标刻度纸1和三只激光笔作为辅助工具测量多关节机械手2的机械手指7的末端位姿，其测量方式为：

1、建立整个系统的基本模型，如图1所示，模型主要由三部分构成：①多关节机械手2的简化模型、②绝对坐标系(x0-y0-z0)和③相对坐标系(xt-yt-zt)，所述坐标刻度纸1的零点0为绝对坐标系的原点，多关节机械手2的竖直臂3设于绝对坐标系的原点上，所述竖直臂3上设有第一转动臂4，所述第一转动臂4上设有第二转动臂5，第二转动臂5上设有第三转动臂6，所述第三转动臂6上设有机械手指7，所述机械手指7的末端设为相对坐标系的原点p，模型上所有点的坐标都是在绝对坐标系中表示的。

2、三只激光笔的上端固定在机械手指7末端上即相对坐标系的原点p，调节好两两激光笔之间的三个夹角α(属于已知量，经测量得出)，使得三个夹角α均相等，如图1、图2所示。

3、三只激光笔向下发射的激光束分别照射在坐标刻度纸1上，照射点分别为a点、b点和c点，在坐标刻度纸1上测量出三个坐标点a、b、c的坐标值，如图1所示。

4，a、b、c三坐标点和p点之间组成三个三角形(组成一个四面体)，再利用正弦定理列出3个约束方程求解机械手指7末端即p点的坐标值，如图1、图2所示。

5、在绝对坐标系和相对坐标系下，利用三个欧拉角ψ、θ、φ建立一个旋转变换矩阵，在旋转变换矩阵中表示有矢量pa、pb、pc。

6、利用旋转变换矩阵表示的矢量pa、pb、pc和绝对坐标系下表示的矢量pa、pb、pc列出三个等式，即为三个关于欧拉角的约束方程，求解三个约束方程即可解得旋转变换矩阵的三个欧拉角。

机械手指7末端位姿由7个参数来确定，分别为机械手指7末端所在相对坐标系(t系)绕绝对坐标系(o系)转动的三个欧拉角ψ、θ、φ，三个激光笔之间的夹角α，相对坐标系原点p在绝对坐标系下的坐标p(x，y，z)，三个欧拉角用来表示机械手指7的末端姿态，三个欧拉角ψ、θ、φ和坐标点p(x、y、z)表示多关节机械手2在运动过程中，机械手指7末端所在的位置和姿态，这一过程和结果运用到多关节机械手2的自动控制程序中，来每时每刻确定和控制机械手指7末端(抓手)的运动，此结果也能作为反馈机制的信号传递给控制系统，形成一个闭环控制系统，对于提高多关节机械手2运动的稳定性和精确性起到很大的作用效果。

7、根据模型构造多关节机械手2的真实结构示意图，真实结构与图1所示建模的多关节机械手2各位置姿态完全一致。

如图3所示，所述多关节机械手2的竖直臂3设于底板8上，所述竖直臂3为螺杆连接的支架，所述支架的转台9上设有控制第一转动臂4转动的第一舵机10，所述第一转动臂4上设置的第二舵机11控制第二转动臂5转动，所述第二转动臂5上设置的第三舵机12控制第三转动臂6转动，所述第三转动臂6设有可进行二维运动的机械手指7(分别由对应的舵机控制，图3中有表示)。

上述结构，相对坐标系(t系)建立在机械手指7末端，点p(x，y，z)代表机械手指7末端在绝对坐标系(o系)下的坐标点值，三个自由转动的关节(对应的舵机所在位置)在不同运动状态时，可以实现机械手指7末端的上下运动、左右运动和前后运动，最终实现多关节机械手2在遥控操作下的各种不同动作。