双目定位多功能机械臂系统设计

Design of Binocular Positioning Multifunctional Manipulator System

摘要: 准确性、快速性与鲁棒性是机械臂系统设计的要求，经过研究提出一套新形式的机器人视觉系统，利用非接触式测量，对所操控单元进行位姿计算，完成操控单元的三维信息与机械臂执行单元进行命令交互。设计采用双目CMOS工业相机，通过Matlab进行相机标定并得到内外参数，然后进行立体校正，结合SGBM立体匹配算法解算出位置深度信息，进而对应帧的二维图像，利用OpenCV视觉算法获取操控器下端标志物的三维坐标，传递执行指令给机械部控制单元。由实验结果分析，在白光环境下该机器人系统同步现实功能展现出良好的跟随准确性，精度误差始终保持在4.5%以内，命令响应时间短，鲁棒性好。

Abstract: Accuracy, rapidity and robustness are the requirements of the design of the manipulator system. After research, a new form of robot vision system is proposed, which uses non-contact measurement to calculate the position and posture of the controlled unit, and completes the command interaction between the three-dimensional information of the control unit and the manipulator execution unit. The design adopts a binocular CMOS industrial camera. The camera is calibrated and the internal and external parameters are obtained through Matlab, and then the stereo correction is carried out. The position depth information is calculated by combining the SGBM stereo matching algorithm, and then the corresponding two-dimensional image of the frame is obtained. The OpenCV vision algorithm is used to obtain the three-dimensional coordinates of the lower marker of the manipulator, and the execution instructions are transmitted to the control unit of the mechanical department. According to the analysis of the experimental results, the synchronous reality function of the robot system shows good tracking accuracy in the white light environment, the accuracy error is always kept within 4.5%, the command response time is short, and the robustness is good.

1. 引言

随着科技与文明的发展，人类对辅助机器人的研究日益深入，逐渐成为机器人研究领域发展的潮流 [1] 。实际工程应用中为了检验零部件与标准数据的偏差需要对零部件实物进行三维扫描获得数据与标准数据进行误差分析 [2] 。光学扫描测量目前以精度高、无接触、分辨率高等优点已经成为获得物体外貌数据的常用手段 [3] [4] 。目前应用中的三维扫描仪在单次测量扫描时间长，面积较小，操作复杂等特点 [5] ，双目视觉技术依据双目人眼定位原理对当前视角范围数据测量，以特定标志点为不变量建立自动拼接模型，现实环境三维空间信息的获取，成本较低且测量方式和计算简单。

通过研究，设计了一套由双目相机，Jetson Nano边缘计算操作平台，AHRS系统构成的机器人视觉系统 [6] ，由双目相机对操控单元目标识别进行准确定位，以及AHRS系统对操作平台进行姿态解算来实现，通过Jetson Nano边缘计算操作平台来承载大量且复杂的矩阵以及偏微分计算，接着实时的位姿信息经过坐标换算 [7] ，发送给机械臂，机械臂收到指令后响应指令。

2. 操控平台系统设计

2.1. 视觉系统设计

双目立体视觉是机器视觉设计灵感来自与人类的双眼，基于视觉差的原理并通过多幅图像来较为准确获取物体三维几何信息的方法 [8] 。以双目视觉摄像头为视觉传感器，在自然光的条件下，两个距离固定CMOS摄像头对下端装配红外LED灯的目标操作平台进行图像采集，经过图像算法处理将目标物的三维信息以及红外LED的位置信息传递到机械臂控制系统中做出响应的指令响应。该系统可以分为四个步骤分别是双目标定、立体校正、立体匹配、测距计算。

2.2. 双目视觉三角测距原理

双目通过感知物体图像所呈现的差异来判断前物体的远近，视差越小，物体的距离越远，反之越近，双目视觉距离演示如图1所示。通过三角测量的数学模型计算，即可得出摄像头与物体的距离 [9] 。

双目视觉测量原理图如图2所示，双目摄像头实验平台处于同一水平面O_l和O_r分别是左右摄像头的光心，两者的距离为基线T。C^left和C^right分别为左右两平面的成像平面，P点是目标物体的位置，x^l和x^r分别是目标物体在左右两个摄像机成像平面上的投影，视差为x^l和x^r的距离，以左右相机的光心在成像平面的投影 ${O^{\prime }}_l$ 和 ${O^{\prime }}_{\text{r}}$ 为成像平面的中心则x^l的坐标为(x_l, y_l)，x^r的坐标为(x_r, y_r)，f为左右相机的焦距，由设计参数可知，左右相机的焦距是相同的，Z是P点到摄像头平面的垂直距离，即为最后测算的深度信息结果。

Figure 1. Binocular vision distance demonstration

图1. 双目视觉距离演示图

Figure 2. Binocular vision distance measurement system

图2. 双目视觉测距原理

由三角形相似定理，可得：

$\frac{T}{Z}=\frac{T-\left(x_l-x_r\right)}{Z-f}$ (1)

由计算可得，相机的测得的深度信息为：

$Z=\frac{f\times T}{x_l-x_r}$ (2)

故若要获得深度信息测量值就需要得到视差 $x_l-x_r$ 。

2.3. 坐标变换与相机参数

世界坐标系、相机坐标系、图像坐标系为上述机器人系统三个基本的坐标系。设目标物体下端红外LED在世界坐标系下为(x_w, y_w, z_w)，在相机坐标系下的坐标为(x_c, y_c, z_c),在图像坐标系下的坐标为(x_p, y_p)。世界坐标系利用刚体变换转变为相机坐标系：

$\left[\begin{array}{c}{x}_c\\ y_c\\ z_c\\ 1\end{array}\right]=\left[\begin{array}{cc}R& T\\ 0^{\text{T}}& 1\end{array}\right]\cdot \left[\begin{array}{c}{\text{x}}_w\\ y_w\\ z_w\\ 1\end{array}\right]$ (3)

式中 $R$ 为右相机相对于左相机的旋转矩阵， $T$ 为右相机相对于左相机的平移矩阵， $R$ 可以分为双目相机的合成矩阵R_l和R_r，其中 $R$ ， $T$ 为相机的外参数不受相机影响。

相机坐标系下通过射影变化转变成图像坐标变换码，三维坐标转化为二维坐标。式中f为相机的焦距。

根据三角形相似原理可知：

$\{\begin{array}{l}x=\frac{f}{z_c}{x}_c\\ y=\frac{f}{z_c}{y}_c\end{array}$ (4)

由相机坐标系和图像坐标系的关系可得

$z_c\left[\begin{array}{c}{x}_p\\ y_p\\ 1\end{array}\right]=\left[\begin{array}{cccc}f& 0& 0& 0\\ 0& f& 0& 0\\ 0& 0& 1& 0\end{array}\right]\left[\begin{array}{c}{x}_c\\ y_c\\ z_c\\ 1\end{array}\right]$ (5)

因为在图像处理中通常以像素为单位，与式(5)中数据单位不统一，转换到像素坐标系：

$\begin{array}{c}{z}_c\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=z_c\left[\begin{array}{ccc}1/dx& 0& u_0\\ 0& 1/dy& v_0\\ 0& 0& 0\end{array}\right]\left[\begin{array}{c}{x}_p\\ y_p\\ 1\end{array}\right]\\ =\left[\begin{array}{ccc}1/dx& 0& u_0\\ 0& 1/dy& v_0\\ 0& 0& 0\end{array}\right]\left[\begin{array}{cccc}f& 0& 0& 0\\ 0& f& 0& 0\\ 0& 0& 1& 0\end{array}\right]\left[\begin{array}{cc}R& T\\ 0^{\text{T}}& 1\end{array}\right]\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\\ 1\end{array}\right]\\ =P_1\left[\begin{array}{cc}R& T\\ 0^{\text{T}}& 1\end{array}\right]\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\\ 1\end{array}\right]=P_1{P}_2\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\\ 1\end{array}\right]\end{array}$ (6)

式中P₁为相机内参数，P₂表示外参数。相机内参数包括畸变系数，由于相机成像不可避免存在径向和切向畸变，其中径向畸较多采用泰勒级数来进行描述，通常取前三阶次的系数k₁，k₂，k₃径向畸变取前两阶p₁，p₂ [10] 。设P点图像坐标系矫正后的坐标为(x_distorted, y_distorted)，畸变矫正后的关系式所示为：

$\{\begin{array}{c}{x}_{\text{distorted}}=x\left(1+k_1{r}^2+k_2{r}^4+k_3{r}^6\right)+2p_{1}xy+p_2\left(r^2+2x^2\right)\\ y_{\text{distorted}}=y\left(1+k_1{r}^2+k_2{r}^4+k_3{r}^6\right)+2p_{2}xy+p_1\left(r^2+2y^2\right)\end{array}$ (7)

其中， $r^2=x^2+y^2$ 。

2.4. 双目标定

双目标定是处理三维世界位姿关系和二维图像坐标对应关系的过程，而双目标定的主要目的是求出左右相机的内外参数矩阵以及畸变参数 [11] 。工业双目相机的有效像素为2560 (H) × 800 (V)，像素大小为3.0 μm × 3.0 μm。标定标靶采用的为正方形黑白棋盘格，经过对21对棋盘格的采集，能得到较为准确的标定精度。

利用Matlab的Stereo Camera Calibrator工具箱进行双目标定。图像重投影误差分析如图3所示，结果表明这21对重投影误差的平均值在0.24 piexls。

Figure 3. Analysis of reprojection error of image

图3. 图像重投影误差分析

最后求出相机的内外参数和左右相机的内外参数。

$P_1=\left[\begin{array}{ccc}\text{599}\text{.413}& \text{0}& \text{319}\text{.040}\\ \text{0}& \text{599}\text{.004}& \text{247}\text{.721}\\ \text{0}& \text{0}& \text{1}\end{array}\right]$ (8)

$P_2=\left[\begin{array}{ccc}\text{599}\text{.873}& \text{0}& \text{315}\text{.817}\\ \text{0}& \text{599}\text{.855}& \text{250}\text{.031}\\ \text{0}& \text{0}& \text{1}\end{array}\right]$ (9)

右相机相对于左相机的旋转矩阵 $R$ 和平移矩阵 $T$

$R=\left[\begin{array}{ccc}\text{0}\text{.999}& -0.008& \text{0}\text{.021}\\ \text{0}\text{.002}& \text{0}\text{.999}& \text{0}\text{.002}\\ \text{0}\text{.018}& \text{0}\text{.003}& \text{0}\text{.999}\end{array}\right]$ (10)

$T=\left[\begin{array}{ccc}\text{5}\text{.318}e-\text{04}& \text{7}\text{.153}e-\text{04}& \text{6}\text{.581}e-\text{05}\end{array}\right]$ (11)

左右相机的畸变系数为Dl和Dr

$\text{DI}=\left[\begin{array}{ccc}-0.138& 0.256& -0.258\end{array}\right]$ (12)

$\text{Dr}=\left[\begin{array}{ccc}-\text{0}\text{.056}& -\text{0}\text{.132}& \text{0}\text{.314}\end{array}\right]$ (13)

双目标定后，将采集到的图像根据上式给出的畸变系数进行畸变矫正后进行立体校正 [12] ，把实际中非共面行进行对准的两幅图像矫正为共面行对准，尽可能校正为理想的双目系统。本项目采用Bouguet校正算法，其原理是使两图像中的每一幅重投影次数最小化的同时使观测面积最大化。校正前与校正后的双目传感器如下图4、图5所示。此处采用OpenCV开源库进行立体校正。

Figure 4. Before stereo calibration

图4. 立体校正前

Figure 5. After stereo calibration

图5. 立体校正后

立体匹配使一种根据平面图像来恢复真实场景的深度信息的技术，通常具有匹配代价计算、代价聚合、视差计算、视差细算四个步骤 [13] 。通常在左相机成像图片任意一个像素点为参考点，并在右相机成像图片中寻找与其对应的参考像素点，获得这两个成像像素点的视差，即可作为求物体深度信息的条件之一 [14] 。

结合以上算法，图像数据处理后可以得到操控平台目标物体(无线操控器)下端红外LED的三维坐标，通过AHRS系统可以实时监测操控器的俯仰角、偏航角、横滚角，算出欧拉角与三维坐标反馈给机械臂控制器，使机械臂达到相应位姿。

3. 机械臂主体

机械臂主体利用开源方案Small Robot Arm 3D打印完成，动力系统采用市面最主流的42步进电机与20、57步进电机进行关节运动，谐波减速器进行力矩保持。选用Arduino Mega 2560最小系统板为主控，TB6600驱动步进电机。该系统具有低成本，运算能力强，上手门槛低等优点，通过无线串口连接PC端上位机实现实时监测。机械臂控制系统框图如下图6所示。

Figure 6. Block diagram of robotic arm control system

图6. 机械臂控制系统框图

机械臂通过接收操控者执行器的指令，解算出的相对于机械臂的世界坐标系的位置，以及末端执行器角度方向的指令，进行运动学逆解计算 [15] ，核算出每个电机执行的旋转角度，进而通过脉冲信号控制步进电机进行同步运动，采用Brehansem算法实现多轴联动与多解路径最优规划机械臂系统设计如下图7。

Figure 7. Robotic arm system design

图7. 机械臂系统设计

4. 同步现实示教实验分析

根据上述实验步骤进行操控器端与机械臂端进行同步仿真，以双目摄像机中心为操控台坐标系原点。以主体机械臂准备姿势下端底座所对应位置为机械臂坐标系原点，两个坐标系方向一致，取操纵器下端红外LED为目标物体中心点，不断调整红外LED的位置进行同步现实仿真，取多个三维空间点位，观察机械臂末端执行器下端执行情况，机械臂系统同步现实示教坐标点分析如表1所示：

Table 1. Simultaneous realistic demonstration coordinate point analysis

表1. 机械臂系统同步现实示教坐标点分析

依据上表数据可绘出如图8所示的操控端LED坐标轨迹图和图9机械臂下端末端执行器的轨迹图。

Figure 8. Coordinate trajectory diagram of the control side

图8. 操控端LED坐标轨迹图

Figure 9. End-effector coordinate trajectory diagram

图9. 末端执行器坐标轨迹图

5. 结论

本项目主要围绕双目视觉传感器对手持操控器通过视觉处理算法进行三维位置捕捉，结合手持操控器内的九轴姿态传感器进行AHRS姿态解算，进而得到操控器的位姿，向机械臂控制器单元输入指令，控制器输出多路PWM操控机械臂达到指定的位姿，其中还对双目相机进行标定得到其内外参数，使得到的图像解算数据更加准确，根据内外参数进行了立体校正和立体匹配，进而测出摄像头与目标物体的垂直距离，结合每对图像的成像位置，即可较为准确的计算出目标物体的三维坐标，本项目在标定时不可避免仍然存在误差，但根据表格数据与仿真实验结果来观察，该机械臂系统的同步现实平均跟随误差为4.5%，跟随效果在误差范围之内，具有一定的可行性和实践价值。

NOTES

^*通讯作者。