CN109859275B - 一种基于s-r-s结构的康复机械臂的单目视觉手眼标定方法 - Google Patents

Abstract

本发明公开了一种基于S‑R‑S结构的康复机械臂的单目视觉手眼标定方法，首先建立康复机械臂的运动学模型；然后标定相机的内参矩阵，利用圆弧轨迹分析法求出康复机械臂末端和基座各关节旋转轴在相机坐标系中坐标表示，进一步求得末端和基座坐标系与相机坐标系之间的转换矩阵，再利用靶标与相机坐标系间的转换矩阵求得末端与靶标之间的转换关系，同理求出基座与相机坐标系间的转换矩阵，实现手眼标定；根据标定得到的靶标与末端间的转换矩阵和基座与相机间的转换矩阵，以及康复机械臂转动后靶标与相机间的实时转换矩阵，可以求得末端坐标系与基座坐标系之间的转换矩阵，实现位姿测量；本发明简化了单目测量的系统结构，手眼标定和位姿测量可以同时测量。

Description

一种基于S-R-S结构的康复机械臂的单目视觉手眼标定方法

技术领域

本发明属于移动机器人定位和单目视觉测量技术领域，具体涉及一种基于S-R-S结构的康复机械臂的单目视觉手眼标定方法。

背景技术

机械臂末端位姿的准确测量是机器人定位技术中的关键问题。视觉传感器因价格低廉、使用方便而受到广泛关注，基于视觉传感器的定位方法一直是研究的热点。根据传感器数目的不同，视觉定位方法可分为单目视觉定位、双目视觉(立体视觉)定位和多目视觉(全方位视觉)定位。单目视觉定位仅需要一台摄像机便能完成定位工作，简单易用且适用广泛，无需解决立体视觉中的两摄像机间的标定和特征点的匹配问题，也不会像全方位视觉传感器那样产生很大的畸变。如何在单目视觉条件下，准确完成位姿求解是一个重要的研究方向。

根据相机与机器人的位置关系，机器人视觉系统可以分为： Eye-in-Hand(EIH)和Eye-to-Hand(ETH)。相机安装在机械臂末端，并随机械臂一起运动的视觉系统称为Eye-in-Hand式手眼系统。相机安装在机械臂本体之外的固定位置，且相机不随机械臂运动的视觉系统称为Eye-to-Hand式手眼系统。针对康复机械臂末端位姿的测量，由于机械臂末端的特殊性，不宜安装相机，所以必须采用固定相机的 ETH视觉系统，此系统中在机械臂末端安装靶标来实现位姿测量。视觉系统的首要任务便是确定相机与机械臂之间的空间关系，即手眼标定。典型的手眼标定方法是控制机械臂末端变换至不同位置，利用相机采集不同位置处靶标坐标系与相机坐标系的转换矩阵，再结合机械臂的运动参数，求解方程AX＝XB确定手眼关系。对于该方程的求解，经典的方法是Tsai等人提出的转站法，但该方法需要机械臂移动较大角度才能标定；另外，存在一些几何方法、特征向量、直积等不同的数学方法解决这一问题，但是由于该方程解的非线性和不稳定性，方程的求解也比较复杂。此外，该方程中矩阵B是机械臂运动前后末端位姿变换矩阵，需要准确的机械臂运动学参数来计算，引入了很大的误差来源，而且针对某些没有完整、准确的运动学参数的康复机械臂，传统的手眼标定方法便无法实现。

发明内容

针对上述现有技术中手眼标定方法计算过程复杂的问题，本发明于提出一种基于S-R-S结构的康复机械臂的单目视觉手眼标定方法；该方法无需求解复杂的非线性方程，而且不需要提供全部的28个DH 参数，只需要提供其中d₁和d₇参数，具体技术方案如下：

一种基于S-R-S结构的康复机械臂的单目视觉手眼标定方法，所述方法包括步骤：

S1、建立指定大小自由度康复机械臂的运动学模型，其中，所述康复机械臂中相邻关节的旋转轴之间均相互垂直且相交于一点；

S2、在所述康复机械臂的末端固定连接一标靶，并固定一相机在所述康复机械臂附近；

S3、以设定的姿态和位置摆放标靶，采用相机获取不同姿态和位置摆放所述标靶的照片，并利用解PNP的方法获取所述相机坐标系在所述康复机械臂转动后与所述标靶坐标系的转换矩阵^bT_c；

S4、旋转所述康复机械臂末端单个关节并利用对应关节转动的圆弧轨迹拟合出所述圆弧轨迹的圆心，基于对应所述关节的旋转轴经过所述圆心获得所述旋转轴在所述相机坐标系中的坐标表示，并获取康复机械臂末端坐标系与所述相机坐标系之间的转换矩阵ⁿT_c；

S5、基于所述转换矩阵ⁿT_c求得所述康复机械臂末端坐标系与所述标靶的转换矩阵^bT_n，并转动所述康复机械臂的基座端的单个关节基于步骤S3中原理获取康复机械臂基座坐标系与所述相机坐标系之间的转换矩阵⁰T_c；

S6、基于所述转换矩阵ⁿT_c与所述转换矩阵⁰T_c获取得到所述康复机械臂末端与所述康复机械臂基座之间的转换矩阵ⁿT₀，得到所述康复机械臂末端相对所述康复机械臂基座的位姿，完成所述康复机械臂的手眼标定。

进一步的，步骤S1中，所述运动学模型采用D-H模型构建，并选定所述康复机械臂基座坐标系{F₀}作为世界坐标系{ω}。

进一步的，所述步骤S3包括：

S31、采用张正友标定法获取所述相机坐标系的内参矩阵M₁；

S32、对所述照片进行角点检测，并从检测得到的所有角点中选取四个端点p1、p2、p3、p4，计算p1、p2、p3、p4四个端点在所述照片中的像素坐标P_i(u_i,v_i)；

S33、基于所述标靶的物理长度获得p1、p2、p3、p4四个端点在所述标靶坐标系中的坐标，并根据所述内参矩阵M₁利用解PNP方法分别获得p1、p2、p3、p4四个端点在所述相机坐标系中的坐标 P_c(x_c,y_c,z_c)。

进一步的，步骤S4包括：

S41、转动所述康复机械臂与末端相邻关节，保持其他关节不动，并实时记录p1、p2端点在所述相机坐标系中的坐标，利用所述坐标生成对应的圆弧形轨迹同时拟合出该轨迹的圆心坐标O_p1、圆心坐标 O_p2，与末端相邻的所述关节的旋转轴经过所述圆心坐标O_p1、圆心坐标O_p2，基于所述圆心坐标O_p1和圆心坐标O_p2求得与末端相邻所述关节在所述相机坐标系中的坐标表示，即康复机械臂末端相邻关节连杆坐标系的Y轴；

S42：将所述康复机械臂与末端相邻关节回到初始位置保持不动，转动所述康复机械臂的末端关节，基于步骤S41原理获得末端关节及其相邻关节连杆坐标系的Z轴；

S43、基于所述Y轴和所述Z轴获得所述康复机械臂末端相邻关节的原点O，并根据右手定则获取所述末端关节相邻关节的X轴在所述相机坐标系中的坐标表示；

S44、基于所述X轴和Y轴及Z轴求得所述末端相邻关节与所述相机坐标系的转换矩阵^n-1T_c；

S45、基于所述末端关节坐标原点O_n在所述康复机械臂末端相邻关节连杆坐标系Z轴的正上方，利用平移矩阵得到末端连杆坐标系与所述相机坐标系中的转换矩阵ⁿT_c，并求得所述标靶坐标系与所述康复机械臂末端坐标系的转换矩阵^bT_n。

进一步的，所述方法还包括获取所述康复机械臂基座坐标系{F₀} 作为世界坐标系{ω}与所述相机坐标系的转换矩阵^wT_c和转换矩阵⁰T_c，且^wT_c＝⁰T_c，求解原理与所述步骤S4中所述转换矩阵^bT_n的求解原理相同。

进一步的，所述相机与所述康复机械臂的相对位置保持不变，对应的，所述转换矩阵^bT_n与转换矩阵^wT_c保持不变。

进一步的，步骤S6还包括：

S61、基于所述转换矩阵^bT_n与转换矩阵^wT_c求得所述康复机械臂末端坐标系与所述康复机械臂基座坐标系{F₀}之间的转换矩阵^wT_n；

S62、从所述转换矩阵^wT_n中取出旋转矩阵R，并利用旋转矩阵R 与欧拉角的关系求得所述康复机械臂末端相对所述康复机械臂基座的姿态角，从而求得机械臂末端位姿。

与现有技术相比，本发明的基于S-R-S结构的康复机械臂的单目视觉手眼标定方法有益效果为：本发明简化了单目测量的系统结构，手眼标定和位姿测量可以同时测量，共用一个标靶；简化了手眼标定算法，无需求解复杂的标定方程；而且无需提供康复机械臂运动模型中所有的DH参数，仅需要测量康复机械臂基座端两个相邻关节距离和末端两个相邻关节距离，有效减少了误差的来源。

附图说明

图1为本发明实施例中所述基于S-R-S结构的康复机械臂的单目视觉手眼标定方法的流程图示意；

图2为本发明实施例中七自由度康复机械臂建模图示意；

图3为本发明实施例中康复机械臂中第六关节和第七关节旋转时特征点轨迹示意图。

具体实施方式

为了使本技术领域的人员更好地理解本发明方案，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述。

实施例一

参阅图1，在本发明实施例中，提供了一种基于S-R-S结构的康复机械臂的单目视觉手眼标定方法，所述方法包括步骤：

S1、建立指定大小自由度康复机械臂的运动学模型，其中，康复机械臂中相邻关节的旋转轴之间均相互垂直且相交于一点；并且为了描述康复机械臂末端在基座坐标系中的位姿，本发明中的运动学模型采用D-H模型构建，并选定康复机械臂基座坐标系{F₀}作为世界坐标系{ω}。

S2、在康复机械臂的末端固定连接一标靶，并固定一相机在康复机械臂附近；优选的，本发明的标靶可以是具有四个特征点的平面或者立体标靶，本发明中的相机采用工业相机；当然此仅为本发明的较佳实施例，并不是对本发明方法的限制和固定，可根据实际情况进行选择。

S3、以设定的姿态和位置摆放所述标靶，采用相机获取不同姿态和位置摆放标靶的照片，并利用解PNP的方法获取相机坐标系在康复机械臂转动后与标靶坐标系的转换矩阵^bT_c；具体过程如下：

首先，基于相机获取得到的标靶不同姿态和位置的照片，采用张正友标定法获取相机坐标系的内参矩阵M₁；然后，对所有照片进行角点检测，从检测得到的所有角点中选取四个端点p1、p2、p3、p4，计算p1、p2、p3、p4四个端点在照片中的像素坐标P_i(u_i,v_i)；最后，因为标靶的物理长度可实际测量获取得到，所以可以基于标靶的物理长度获得p1、p2、p3、p4四个端点在标靶坐标系中的坐标，并根据内参矩阵M₁利用解PNP方法分别获得p1、p2、p3、p4四个端点在相机坐标系中的坐标P_c(x_c,y_c,z_c)。

S4、旋转康复机械臂末端单个关节并利用对应关节转动的圆弧轨迹拟合出圆弧轨迹的圆心，基于对应关节的旋转轴经过所述圆心获得旋转轴在相机坐标系中的坐标表示，并获取康复机械臂末端坐标系与相机坐标系之间的转换矩阵ⁿT_c；具体包括步骤：

首先，转动康复机械臂与末端相邻关节，保持其他关节不动，并实时记录p1、p2端点在相机坐标系中的坐标，利用坐标生成对应的圆弧形轨迹同时拟合出该轨迹的圆心坐标O_p1、圆心坐标O_p2，与末端相邻的关节的旋转轴经过圆心坐标O_p1、圆心坐标O_p2，基于圆心坐标O_p1和圆心坐标O_p2求得与末端相邻所述关节在相机坐标系中的坐标表示，即康复机械臂末端相邻关节连杆坐标系的Y轴；然后，将康复机械臂与末端相邻关节回到初始位置保持不动，转动康复机械臂的末端关节，基于上述关于Y轴坐标获取原理求得末端关节及其相邻关节连杆坐标系的Z轴；随后，基于求得的Y轴和Z轴获取末端关节及其相邻关节连杆坐标系的原点O，并根据末端关节的坐标原点O_n在康复机械臂末端相邻关节连杆坐标系的原点O的正上方求得坐标原点O_n的在相机坐标系中的坐标表示；最后，基于坐标原点O_n及平行原理获取末端关节的Y_n轴在相机坐标系中的坐标表示，并根据右手定则获取末端关节的X_n轴在所述相机坐标系中的坐标表示；由此，基于Y_n轴和X_n轴及坐标原点O_n就可求得转换矩阵ⁿT_c，并求得标靶坐标系与康复机械臂末端坐标系的转换矩阵^bT_n。

S5、基于转换矩阵ⁿT_c求得康复机械臂末端坐标系与标靶的转换矩阵^bT_n，并转动康复机械臂的基座端的单个关节基于步骤S3中原理获取康复机械臂基座坐标系与相机坐标系之间的转换矩阵⁰T_c；同时，获取康复机械臂基座坐标系{F₀}作为世界坐标系{ω}与相机坐标系的转换矩阵^wT_c和转换矩阵⁰T_c，且^wT_c＝⁰T_c，所述求解原理与步骤S4中所述转换矩阵^bT_n的求解原理相同，在此不再进行赘述。

其中，在本发明中，相机与康复机械臂的相对位置保持不变，即康复机械臂在转动过程中也和相机的相对位置保持不变；由此可知，转换矩阵^bT_n与转换矩阵^wT_c保持不变。

S6、基于转换矩阵ⁿT_c与转换矩阵⁰T_c获取得到康复机械臂末端与康复机械臂基座之间的转换矩阵ⁿT₀，得到康复机械臂末端相对康复机械臂基座的位姿，完成康复机械臂的手眼标定；具体包括步骤如下：

先基于转换矩阵^bT_n与转换矩阵^wT_c求得康复机械臂末端坐标系与康复机械臂基座坐标系{F₀}之间的转换矩阵^wT_n；再从转换矩阵^wT_n中取出旋转矩阵R并利用旋转矩阵R与欧拉角的关系求得康复机械臂末端相对康复机械臂基座的姿态角，并结合S4中获取得到的坐标原点 O_n在康复机械臂基座坐标系中的位置，即可求得机械臂末端位姿。

实施例二

下面结合图2和图3，以构建七自由度的康复机械臂来对本发明的方法进行详细说明，具体包括步骤如下：

步骤一：构建含有q1、q2、…、q7七个转动关节的康复机械臂运动模型，其中，关节q1与关节q2垂直相交于一点、关节q2与关节q3垂直相交于一点、……，关节q6与关节q7垂直相交于一点， X₀～X_n、Z₀～Z_n分别为机械臂基座坐标系、第一、二、…、七连杆坐标系、末端坐标系的X轴和Z轴，Y轴由右手定则可以得到；选定机械臂基座坐标系作为世界坐标系{ω}。

步骤二、在康复机械臂末端固联一块棋盘格作为标靶，并将工业相机固定在康复机械臂附近，确保康复机械臂转动过程中工业相机与康复机械臂基座相对位置不变；然后，调整光圈、焦距使工业相机在康复机械臂转动过程中尽可能采集到清晰的图片，并对相机进行标定；具体采用张正友标定法，将标定用的棋盘格在标靶附近以不同姿态、位置摆放，利用相机采集一组照片，然后利用张正友标定法标定方法进行棋盘格的标定，得到标靶的内参矩阵M₁。

步骤三、工业相机标定完成后，采集初始状态下含有标靶的康复机械臂末端照片，对采集的照片利用Harris算法进行角点检测，从检测到的角点集合中取出棋盘格的四个端点p1、p2、p3、p4，将这几个点作为特征点，可知这些点的像素坐标P_i(u_i,v_i)，由棋盘格物理距离可知这些点在棋盘格坐标系{b}中坐标P_b(x_b,y_b,z_b)，然后根据公式：

式中除了外参矩阵M₂都是已知的，可以利用三种方法求解外参矩阵：利用DLT方法直接将四个端点p1、p2、p3、p4代入求解线性方程组求解外参矩阵中12个未知数；也可以先将其中三个特端点使用P3P方法求解出四种可能的姿态，再用第四个端点的投影去除三个干扰解；还有种EPNP方法，通过n个3D点在工业相机平面的投影关系，以及与这四个端点p1、p2、p3、p4的权重关系，构建一个12*12 方阵，求得其零空间特征向量，可以得到虚拟控制点的相机平面坐标，然后使用POSIT算法即可求出工业相机位姿，通常在用EPnP求得四对点下的封闭解后，可以将该解作为非线性优化的初值，优化提高精度；求得的外参矩阵M₂即棋盘格坐标系{b}与工业相机坐标系{C}之间的转换矩阵^bT_c，由此可以得到各个特征点在相机坐标系中的坐标 P_c(x_c,y_c,z_c)。

步骤四、结合图3,，首先，仅转动康复机械臂的第六关节电机，保持其他关节不动，并记录p1、p2在相机坐标系中的坐标，因为特征点相对康复机械臂位置是不变的，所以在第六关节转动过程中，点p1、p2的轨迹是两条圆弧，图中p1’、p1”分别为第六关节旋转不同角度处的p1点，可以求得对应的点p1_i，表示第i个位置对应的点 p1在相机坐标系{C}中的坐标表示为：

同时，由于误差的存在，这些点必然不会完全共面，此处的圆弧轨迹是个近似的空间圆；先利用n个点拟合出空间圆所在平面，可以设此平面方程为： ax+by+cz-1＝0

即：

利用最小二乘法可得X＝(A^TA)^-1A^Tl；因为圆上任意两点连线垂直于该线中点与圆心的连线，可以得到：

令

则有：

x_ijx+y_ijy+z_ijz-L_i＝0

将其与式

联立，由间接平差原理可得：

由最小二乘法的参数解为

X＝(B^TPB)^-1B^TPL

式中P为观测值的权，因为是等精度选取的点，所以根据圆上点的数量令P为n*n的对角矩阵P＝diag(1 1 L 1)_n个1。公式 X＝(B^TPB)^-1B^TPL求得圆弧1对应的圆心在相机坐标系{C}中的坐标表示O_p1(x,y,z)，同理可以求得圆弧2对应的圆心在相机坐标系{C}中的坐标表示O_p2,因为第六关节的旋转轴J6必定经过这两个圆心，从而可以求得

在相机坐标系{C}中的坐标表示，单位化后即第七连杆坐标系{F₇}的Y₇轴：

步骤五、求得第七连杆坐标系Y轴在相机坐标系{C}中的坐标表示后将第六关节回到初始位置保持不动，仅转动第七关节，利用图3 所示的p1和p4点同理可以求得J7轴在相机坐标系{C}系中的坐标表示，即第七连杆坐标系{F₇}的Z₇轴，则Y₇轴与Z₇轴的交点为{F₇}系的原点O₇，因为存在误差不一定正好有交点，具体求法可以在Z₇轴上取一点使其到Y₇轴距离最短，而X₇轴可以利用右手定则求得。

步骤六、由运动模型可知末端坐标系{F_n}的原点在轴上距离圆心d₇处，三个坐标轴与第七连杆坐标系平行，即将{F₇}沿Z轴平移d₇即可得到末端坐标系：

{F_n}＝Trans(0,0,d₇){F₇}，即第七连杆坐标系三个坐标轴在相机坐标系 {C}中的坐标表示皆已知，设为n，o，a，则末端坐标系{F_n}与相机坐标系之间的转换关系为：

最后利用{b}系与相机坐标系{C}系之间的转换矩阵^bT_c，可以求得棋盘格坐标系{b}与末端坐标系{F₇}的转换矩阵^bT_n＝^bT_c(ⁿT_c)^-1。

步骤七、同理分别旋转第一关节和第二关节可以得到康复机械臂基座端的第一关节的旋转轴J1和第二关节的旋转轴J2在相机坐标系 {C}中坐标表示，再利用d₁可以求得Z₀和Y₀，然后利用右手定则求得X₀，从而得到基座标系即世界坐标{ω}与相机坐标系{C}的转换矩阵^wT_c，从而实现了康复机械臂的手眼标定过程。

步骤八、进行康复机械臂末端位姿测量：已知初始状态下棋盘格坐标系{b}与机械臂末端坐标系{F_n}的转换矩阵^bT_n，世界坐标系{ω} 与相机坐标系{C}的转换矩阵^wT_c，因为棋盘格是固联在康复机械臂末端的，工业相机和康复机械臂基座的相对位置也是不变的，所以当机械臂转动时转换矩阵^bT_n和转换矩阵^wT_c保持不变；当康复机械臂转动后，利用工业相机采集的照片信息，然后检测棋盘格角点，再利用角点在棋盘格坐标系{b}系中的坐标，以及已经标定得到的工业相机内参矩阵M₁，利用解PNP方法可以得到外参矩阵即棋盘格坐标系{b}与相机坐标系{C}之间的转换矩阵^bT'_c，然后利用不变的转换矩阵^bT_n和转换矩阵^bT_n，可以求得康复机械臂转动后末端坐标系与世界坐标系{ω} 即康复机械臂基座坐标系之间的转换矩阵ⁿT'_w＝(^bT_n)^-1bT'_c(^wT_c)^-1；

步骤九、从转动后的转换矩阵^wT'_n中提取出旋转矩阵R，利用如下公式：

所示旋转矩阵R与欧拉角的关系可以求得康复机械臂末端相对基座的姿态角，并结合得到的坐标原点O₇在世界坐标系{ω}中的位置，从而求得康复机械臂末端实时位姿。

最后，为了验证本发明方法的有效性，基于上述步骤一至步骤九，以七自由度康复机械臂为里进行说明，并在末端固联一块含有高精度棋盘格的平板，同时，固定工业相机在机械臂附近，调整好工业相机的位置、光圈等进行实验，具体过程如下：

首先，利用机械臂改变棋盘格位姿，采集20张照片标定出相机内参矩阵M₁，从而求得初始状态下棋盘格坐标系与相机坐标系的转换矩阵，然后利用圆弧轨迹法拟合圆心求出末端坐标系与相机坐标系的转换矩阵，以及基座与相机坐标系的转换矩阵，然后可以求得初始状态下机械臂末端与基座的转矩阵为

根据公式

计算得到初始末端欧拉角为

[-1.6 2.0582 -0.6646]^T。

为了验证手眼标定的有效性，利用高精度的关节臂测量仪测得机械臂初始状态的DH参数，然后计算得到相对精确的末端相对基座的旋转矩阵为

计算得到真实的末端欧拉角为[0 0 0]^T，误差并不大，考虑到仿真与机械臂实际位姿间的误差，可以说明本发明中的手眼标定方法有效。最后任意旋转机械臂各个关节角度，利用相对精确的DH参数计算的实时末端位姿作为真实值，利用本发明方法测得的末端位姿作为测量值，进行了五组实验，测量误差如表1所示，从中可知，本发明位姿测量精度可以满足康复机械臂要求。

表1位姿测量误差

本发明的基于S-R-S结构的康复机械臂的单目视觉手眼标定方法，通过建立指定数值大小自由度的康复机械臂，在康复机械臂上设置随康复机械臂转动不会改变两者相对位置的相机，并以相机建立对应的坐标系，然后通过分别转动康复机械臂末端相邻两关节的方式获得康复机械臂末端相对于相机坐标系的坐标表示，以及通过分别转动康复机械臂基座端相邻两个关节来计算获得康复机械臂基座端相对于相机坐标系的坐标表示，计算康复机械臂末端坐标系和基座坐标系之间的位姿，实现手眼标定；与现有技术相比，本发明的有益效果为：简化了单目测量的系统结构，手眼标定和位姿测量可以同时测量，共用一个标靶；简化了手眼标定算法，无需求解复杂的标定方程；而且无需提供康复机械臂运动模型中所有的DH参数，仅需要测量康复机械臂基座端两个相邻关节距离和末端两个相邻关节距离，有效减少了误差的来源。

以上仅为本发明的较佳实施例，但并不限制本发明的专利范围，尽管参照前述实施例对本发明进行了详细的说明，对于本领域的技术人员来而言，其依然可以对前述各具体实施方式所记载的技术方案进行修改，或者对其中部分技术特征进行等效替换。凡是利用本发明说明书及附图内容所做的等效结构，直接或间接运用在其他相关的技术领域，均同理在本发明专利保护范围之内。

Claims (7)

1.一种基于S-R-S结构的康复机械臂的单目视觉手眼标定方法，其特征在于，所述方法包括步骤：

S1、建立指定大小自由度康复机械臂的运动学模型，其中，所述康复机械臂中相邻关节的旋转轴之间均相互垂直且相交于一点；

S2、在所述康复机械臂的末端固定连接一标靶，并固定一相机在所述康复机械臂附近；

S3、以设定的姿态和位置摆放标靶，采用相机获取不同姿态和位置摆放所述标靶的照片，并利用解PNP的方法获取相机坐标系在所述康复机械臂转动后与标靶坐标系的转换矩阵^bT_c；

S4、旋转所述康复机械臂末端单个关节并利用对应关节转动的圆弧轨迹拟合出所述圆弧轨迹的圆心，基于对应所述关节的旋转轴经过所述圆心获得所述旋转轴在所述相机坐标系中的坐标表示，并获取康复机械臂末端坐标系与所述相机坐标系之间的转换矩阵ⁿT_c；

S5、基于所述转换矩阵ⁿT_c求得所述康复机械臂末端坐标系与所述标靶的转换矩阵^bT_n，并转动所述康复机械臂的基座端的单个关节基于步骤S3中原理获取康复机械臂基座坐标系与所述相机坐标系之间的转换矩阵⁰T_c；

S6、基于所述转换矩阵ⁿT_c与所述转换矩阵⁰T_c获取得到所述康复机械臂末端与所述康复机械臂基座之间的转换矩阵ⁿT₀，得到所述康复机械臂末端相对所述康复机械臂基座的位姿，完成所述康复机械臂的手眼标定。

2.如权利要求1所述的基于S-R-S结构的康复机械臂的单目视觉手眼标定方法，其特征在于，步骤S1中，所述运动学模型采用D-H模型构建，并选定所述康复机械臂基座坐标系{F₀}作为世界坐标系{ω}。

3.如权利要求2所述的基于S-R-S结构的康复机械臂的单目视觉手眼标定方法，其特征在于，所述步骤S3包括：

S31、采用张正友标定法获取所述相机坐标系的内参矩阵M₁；

S32、对所述照片进行角点检测，并从检测得到的所有角点中选取四个端点p1、p2、p3、p4，计算p1、p2、p3、p4四个端点在所述照片中的像素坐标P_i(u_i,v_i)；

S33、基于所述标靶的物理长度获得p1、p2、p3、p4四个端点在所述标靶坐标系中的坐标，并根据所述内参矩阵M₁利用解PNP方法分别获得p1、p2、p3、p4四个端点在所述相机坐标系中的坐标P_c(x_c,y_c,z_c)。

4.如权利要求3所述的基于S-R-S结构的康复机械臂的单目视觉手眼标定方法，其特征在于，步骤S4包括：

S41、转动所述康复机械臂与末端相邻关节，保持其他关节不动，并实时记录p1、p2端点在所述相机坐标系中的坐标，利用所述坐标生成对应的圆弧形轨迹同时拟合出该轨迹的圆心坐标O_p1、圆心坐标O_p2，与末端相邻的所述关节的旋转轴经过所述圆心坐标O_p1、圆心坐标O_p2，基于所述圆心坐标O_p1和圆心坐标O_p2求得与末端相邻所述关节在所述相机坐标系中的坐标表示，即康复机械臂末端相邻关节连杆坐标系的Y轴；

S42：将所述康复机械臂与末端相邻关节回到初始位置保持不动，转动所述康复机械臂的末端关节，基于步骤S41原理获得末端关节及其相邻关节连杆坐标系的Z轴；

S43、基于所述Y轴和所述Z轴获得所述康复机械臂末端相邻关节的原点O，并根据右手定则获取所述末端关节相邻关节的X轴在所述相机坐标系中的坐标表示；

S44、基于所述X轴和Y轴及Z轴求得所述末端相邻关节与所述相机坐标系的转换矩阵^{n- 1}T_c；

S45、基于所述末端关节坐标原点O_n在所述康复机械臂末端相邻关节连杆坐标系Z轴的正上方，利用平移矩阵得到末端连杆坐标系与所述相机坐标系中的转换矩阵ⁿT_c，并求得所述标靶坐标系与所述康复机械臂末端坐标系的转换矩阵^bT_n。

5.如权利要求4所述的基于S-R-S结构的康复机械臂的单目视觉手眼标定方法，其特征在于，所述方法还包括获取所述康复机械臂基座坐标系{F₀}作为世界坐标系{ω}与所述相机坐标系的转换矩阵^wT_c和转换矩阵⁰T_c，且^wT_c＝⁰T_c，求解原理与所述步骤S4中所述转换矩阵^bT_n的求解原理相同。

6.如权利要求5所述的基于S-R-S结构的康复机械臂的单目视觉手眼标定方法，其特征在于，所述相机与所述康复机械臂的相对位置保持不变，对应的，所述转换矩阵^bT_n与转换矩阵^wT_c保持不变。

7.如权利要求6所述的基于S-R-S结构的康复机械臂的单目视觉手眼标定方法，其特征在于，步骤S6还包括：

S61、基于所述转换矩阵^bT_n与转换矩阵^wT_c求得所述康复机械臂末端坐标系与所述康复机械臂基座坐标系{F₀}之间的转换矩阵^wT_n；

S62、从所述转换矩阵^wT_n中取出旋转矩阵R，并利用旋转矩阵R与欧拉角的关系求得所述康复机械臂末端相对所述康复机械臂基座的姿态角，从而求得机械臂末端位姿。

Images