一种视觉-角度计融合的并联机器人隔空随动控制方法

本发明涉及的机器人控制，尤其涉及一种视觉-角度计融合的并联机器人隔空随动控制方法。

背景技术：

1、随着工业自动化及智能制造的发展，机器人在分拣、搬运、装配及教学实验等场景中的应用日益广泛。示教作为创建机器人作业程序的主要手段，其便捷性与直观性直接影响生产效率。

2、目前，主流的机器人示教方式主要包括直接示教、离线示教和视觉引导示教。直接示教通常依赖示教器或人工拖动机器人末端执行器，操作者需具备一定的机器人坐标系与运动学基础。这种方法不仅示教效率低，且操作者直接接触运动机构，存在安全隐患；

3、同时，对于三轴并联机器人这类高速、轻量化的并联机构，拖动示教易受机构刚度与摩擦力的干扰，难以获得平滑的轨迹。离线示教则对操作者的计算机图形学能力要求较高，且虚拟仿真与实际作业环境往往存在误差，难以满足复杂工况的需求。

4、近年来，基于机器视觉的非接触式示教技术逐渐成为研究热点。现有技术中，通常采用摄像头采集操作者手部图像，利用图像处理算法识别手部位置或手势类别，进而控制机器人运动。然而，此类技术在实际应用中仍存在以下亟待解决的技术问题：

5、第一，三维空间坐标获取困难或成本高昂。普通单目摄像头仅能稳定获取图像平面内的二维位置信息（x、y坐标），难以直接获取垂直于成像平面的深度信息（z坐标）。若采用图像面积变化推算深度，极易受光照、遮挡及手部姿态变化的影响，导致数据抖动剧烈。若采用深度相机（如rgb-d相机）或数据手套来获取三维坐标，虽能提升精度，但会显著增加硬件成本、标定复杂度及系统部署难度。

6、第二，运动轨迹连续性差。现有视觉示教方案多采用离散手势识别逻辑，即通过识别张手、握拳、指向等特定手势，控制机器人执行启动、停止或单步移动。这类方法虽然能够实现基本的人机交互控制，但其输出通常是有限类别的控制命令，无法直接表达操作者手部连续运动过程中的空间位移变化。因此，在机器人示教过程中，这种“指令-执行”模式导致机器人运动呈阶跃式变化，无法复现操作者手部的自然连续动作。对于三轴并联机器人而言，输入的频繁跳变极易引发末端抖动、轨迹突变，严重影响示教轨迹的平顺性与示教过程的稳定性。

7、针对上述问题，中国专利申请cn106020494a公开了一种基于移动跟踪的三维手势识别方法，其将leap motion体感控制器固定于机器人末端以获取手部深度信息，但该方案增加了机器人末端负载，限制了运动灵活性，且其依赖复杂的串联机器人运动学算法，不适用于并联机器人控制架构。中国专利申请cn206326605u公开了一种基于机器视觉的智能示教系统，其采用双目视觉设备获取深度信息，但同样存在硬件成本高、系统架构复杂的问题，且未涉及针对视觉信号抖动的底层控制优化机制。

8、因此，如何提供一种成本低廉、架构简洁，且能实现机器人末端对操作者手部动作进行连续、平滑跟随的非接触示教控制方法，是本领域亟需解决的技术难题。

技术实现思路

1、本发明的目的在于提供一种视觉-角度计融合的三轴并联机器人随动控制方法，旨在提供一种成本低廉、架构简洁，且能实现机器人末端对操作者手部动作进行连续、平滑跟随的非接触示教控制方法。

2、为实现上述目的，提供一种视觉-角度计融合的并联机器人隔空随动控制方法，包括以下步骤：

3、s1、构建闭环控制系统：构建由视觉采集单元、角度传感单元、上位机数据处理单元及工业控制器执行单元构成的闭环控制系统；

4、其中，视觉采集单元配置为采集操作者手部图像；

5、角度传感单元配置为固定于操作者手腕以采集俯仰角度；

6、上位机数据处理单元与视觉采集单元及角度传感单元通信连接；

7、工业控制器执行单元与上位机数据处理单元通过工业以太网协议进行双向数据交互，且其输出端通过实时总线与三轴并联机器人的三个伺服驱动器电性连接；

8、s2、生成三维目标坐标：通过视觉采集单元获取操作者手部图像，利用手部骨架关键点提取算法识别手部骨架并提取手腕关键点的归一化坐标，将归一化坐标映射获得机器人工作平面内的第一坐标信息，同时通过角度传感单元获取操作者手腕的俯仰角度，并将俯仰角度经非线性映射模型转换为机器人末端的第二坐标信息；

9、s3、数据帧生成：将第一坐标信息与第二坐标信息进行时间同步融合，并对融合后的三维坐标进行一阶低通滤波处理及工作空间有效性判断，生成包含三维目标位置及有效性标志位的数据帧；

10、s4、数据帧传输：建立上位机数据处理单元与工业控制器执行单元之间的通信连接，将数据帧发送至工业控制器执行单元；

11、s5、锁存与逆运动学求解：在工业控制器执行单元中，按照预设的锁存周期对接收到的三维目标位置进行锁存，将锁存后的目标位置输入预置的逆运动学求解单元以计算获得各主动支链的目标关节角，并将目标关节角转换为伺服位置指令后下发至伺服驱动器，驱动机器人末端跟随操作者手部动作运动。

12、在其中一个技术方案中，步骤s2中将归一化坐标映射获得机器人工作平面内的第一坐标信息，具体包括：

13、上位机数据处理单元根据视觉采集单元的图像宽度和高度，将归一化坐标转换为实际像素坐标；

14、随后以图像中心为原点建立相机平面坐标系，将手腕关键点的像素坐标转换为相机平面坐标(xc, yc)，其中，xc、yc分别为以图像中心为原点的相机平面坐标系中的的x轴、y轴的坐标；

15、通过透视变换矩阵hm将相机平面坐标映射为机器人工作平面坐标，设定转换后的相机平面坐标的齐次形式为，其中，pc为以图像中心为原点的相机平面坐标的齐次形式；

16、经透视变换后的齐次坐标为，则机器人工作平面坐标为(xr, yr)，机器人工作平面坐标满足以下透视关系：,，透视关系用于消除摄像头倾斜安装带来的透视畸变；

17、其中，xr和yr分别为机器人工作平面中的x轴坐标与y轴坐标，δx和δy分别为x轴方向与y轴方向的调试偏置量，和分别为透视变换后齐次坐标的第一分量、第二分量，为随相机平面坐标位置变化而动态变化的缩放倍数。

18、在其中一个技术方案中，步骤s2中将俯仰角度经非线性映射模型转换为机器人末端的第二坐标信息，具体包括：

19、上位机数据处理单元调用角度传感单元的在线接口，读取原始返回值draw+4000，draw为角度传感单元通过在线接口返回的原始数字量；

20、由于该原始返回值包含固定偏置4000，先进行偏置修正：dadc=draw+4000−4000；

21、其中，dadc为偏置修正后的传感器数值；

22、再根据比例系数kd将dadc换算为初始角度值θd：θd=dadc·kd；

23、其中，θd为角度传感单元输出的初始角度值，kd为传感器比例系数，且kd=0.045；

24、对θd进行安全限幅处理，获得当前俯仰角度θc；

25、将限幅后的θc代入二次映射模型，计算得到机器人末端的z轴绝对坐标zt：zt=−0.0030219780·θc2−0.9313186813·θc−420.0；

26、其中，zt为映射得到的三轴并联机器人末端z轴目标坐标，该二次映射模型基于三组标定约束点拟合获得：当θc=−60°时zt=−375mm，当θc=0°时zt=−420mm，当θc=70°时zt=−500mm。

27、在其中一个技术方案中，步骤s3中对融合后的三维坐标进行一阶低通滤波处理，具体采用一阶低通滤波算法，三维坐标包括x轴坐标x、y轴坐标y和z轴坐标z，分别对x、y、z独立执行滤波运算，其滤波公式为：

28、；

29、其中，、、为当前时刻原始输入x轴、y轴、z轴坐标值，、、分别为当前时刻滤波后的输出x轴、y轴、z轴坐标值，、、为上一时刻滤波后的输出x轴、y轴、z轴坐标值，α为滤波系数，且0<α<1。

30、在其中一个技术方案中，步骤s3中工作空间有效性判断具体包括：

31、根据滤波后的平面坐标(xf(k), yf(k))计算目标点相对于工作空间中心的平面半径r：

32、；

33、设定工作空间最大允许半径rmax；

34、当r≤rmax且z轴坐标zf(k)处于预设的安全高度范围内时，生成有效性标志位valid=1；

35、否则生成有效性标志位valid=0，禁止机器人运动。

36、在其中一个技术方案中，步骤s5中按照预设的锁存周期对接收到的三维目标位置进行锁存，具体包括：

37、设定锁存分频系数n与工业控制器的plc扫描周期ts，确定目标点锁存周期tl：tl=n·ts；

38、其中，ts为plc扫描周期，n为锁存分频系数，tl为目标点锁存周期；

39、仅在每一个锁存周期tl到来时，将上位机数据处理单元发送的最新三维目标位置(xnet(k), ynet(k), znet(k))更新为当前逆运动学计算所用的目标坐标(xcmd(k), ycmd(k),zcmd(k))，在锁存周期间隔内保持目标坐标恒定；

40、其中，xnet(k)、 ynet(k)、 znet(k)分别为上位机数据处理单元在第k个锁存周期内发送的x轴、y轴、z轴的目标位置，xcmd(k), ycmd(k), zcmd(k)分别为工业控制器执行单元在第k个锁存周期内锁存后用于逆运动学计算的x轴、y轴、z轴目标坐标。

41、在其中一个技术方案中，plc扫描周期ts为4ms，锁存分频系数n为25，目标点锁存周期tl为100ms。

42、在其中一个技术方案中，并联机器人包括三条主动支链，步骤s5中将目标关节角转换为伺服位置指令，具体包括：

43、设定各主动支链伺服轴的安装方向系数di与零点偏置oi，其中i=1,2,3分别对应三轴并联机器人的三条主动支链；

44、预先将各主动支链的主动臂处于水平状态时的位置设定为对应主动关节的机械零位，此时对应主动关节角为0°；

45、第k个锁存周期内，第i条主动支链的目标关节角θi(k)为该主动支链的主动臂轴线与其转动平面内水平基准线之间的有向夹角；

46、其中，主动臂处于水平状态时，θi(k) = 0°，主动臂由水平位置向下转动时，θi(k)为正值，主动臂由水平位置向上转动时，θi(k)为负值；

47、根据公式将第i轴的目标关节角θi(k)转换为第i轴伺服位置指令pi(k)：pi(k)=di·θi(k)+oi；

48、其中，k为锁存周期序号，pi(k)为第i个伺服轴在第k次锁存周期的目标位置指令，di为第i轴伺服方向系数，用于修正伺服电机正转方向与机器人末端运动方向的一致性，oi为第i轴零点偏置，用于补偿伺服轴零点与机器人机械零位的偏差；

49、并通过实时总线下发至对应的伺服驱动器。

50、在其中一个技术方案中，工业控制器执行单元在调用逆运动学求解单元时，以上一锁存周期计算得到的合法关节角作为当前逆解运算的初始参考值，以减少逆解分支切换造成的运动不连续。

51、在其中一个技术方案中，步骤s4中建立通信连接，具体包括：工业控制器执行单元通过通信通道向上位机数据处理单元发送视觉使能信号；

52、上位机数据处理单元根据视觉使能信号启动或关闭图像采集与坐标解算进程；

53、上位机数据处理单元按固定数据格式封装并发送数据帧，固定数据格式为"x,y,z,valid"；

54、其中，x为机器人工作平面x轴坐标，y为机器人工作平面y轴坐标，z为机器人末端z轴绝对坐标，valid为有效性标志位。

55、实施本发明技术方案，将具有如下有益效果：

56、本发明公开的视觉角度计融合的三轴并联机器人非接触随动控制方法，是针对三轴并联机器人示教场景设计的低成本、高连续性非接触控制方案，旨在解决现有单目视觉无法稳定获取深度信息、深度相机硬件成本高企、离散手势控制导致轨迹阶跃抖动的技术痛点，整体构建由视觉采集单元、角度传感单元、上位机数据处理单元、工业控制器执行单元组成的闭环控制链路，其中视觉采集单元可选用经内参标定的ccd或cmos单目工业相机、高清网络摄像头等具备图像采集能力的设备，仅需满足不低于25帧/秒的采集帧率即可适配示教场景的动态响应需求，角度传感单元可选用datalite无线角度传感器、微型mems惯性测量单元、贴片式倾角传感器等可固定于操作者手腕的俯仰角检测设备，重量通常控制在50g以内以避免对操作者手部动作灵活性造成干扰，上位机数据处理单元可选用搭载python运行环境的工业控制计算机、高性能pc、树莓派4b或nvidia jetson nano等具备浮点运算能力的嵌入式设备，工业控制器执行单元可选用基于codesys平台的软plc、西门子s7-1500系列可编程逻辑控制器、倍福cx2040嵌入式控制器、基于stm32h743芯片自主开发的运动控制板卡等具备实时运算能力的控制设备，各单元之间可通过usb3.0、千兆以太网、蓝牙5.0、wifi6等有线或无线链路实现数据交互，工业控制器执行单元与三轴delta并联机器人的三个伺服驱动器之间可通过ethercat、profinet、powerlink、sercos iii等实时工业总线实现指令传输，工作时首先完成系统搭建与各单元的参数标定，视觉采集单元实时采集包含操作者手部的图像，上位机数据处理单元调用mediapipe hand model、openpose、hrnet等任意一种手部骨架关键点提取算法识别手部骨架，提取手腕关键点的归一化坐标后，先根据图像分辨率转换为实际像素坐标，再以图像中心为原点建立相机平面坐标系，通过预先标定得到的3×3透视变换矩阵将相机平面坐标映射为机器人工作平面内的x轴、y轴第一坐标信息，同时角度传感单元实时采集操作者手腕的俯仰角度，上位机数据处理单元先对传感器原始返回值进行偏置修正以消除出厂固有偏差，再按照0.045的比例系数换算为初始角度值后进行安全限幅，避免传感器异常跳变值进入后续流程，之后将限幅后的角度值代入预先通过三组标定约束点拟合得到的二次映射模型，计算得到机器人末端的z轴第二坐标信息，随后将第一坐标信息与第二坐标信息进行时间同步融合得到三维目标坐标，对该三维坐标独立执行一阶低通滤波运算以消除手部自然抖动、图像识别噪声、传感器瞬时波动带来的数据跳变，再根据滤波后的平面坐标计算目标点相对于工作空间中心的平面半径，结合z轴坐标是否处于预设安全高度范围判断目标点有效性，生成包含三维目标位置及有效性标志位的数据帧，通过udp、tcp、modbus tcp等任意一种工业以太网协议将数据帧传输至工业控制器执行单元，工业控制器执行单元按照预设的锁存周期对接收到的三维目标位置进行锁存，锁存周期可通过设定plc扫描周期与锁存分频系数的乘积灵活调整，例如采用4ms的plc扫描周期搭配25倍的锁存分频系数即可得到100ms的目标点锁存周期，仅在锁存周期到来时将最新的三维目标位置更新为逆运动学计算的输入目标坐标，锁存周期间隔内保持目标坐标恒定，之后调用以解析法或迭代法实现的逆运动学求解单元，以上一锁存周期计算得到的合法关节角作为当前逆解运算的初始参考值，计算得到三轴并联机器人的三条主动支链对应的目标关节角，再根据各伺服轴的安装方向系数与零点偏置将目标关节角转换为伺服位置指令，通过实时总线下发至伺服驱动器驱动机器人末端运动，从而实现机器人末端对操作者手部动作的连续跟随。在此基础上，本发明的有益效果可通过严密的逻辑推理得到验证：针对现有技术中单目视觉无法获取稳定深度信息、深度相机成本过高的痛点，本发明采用视觉采集单元获取平面坐标、角度传感单元获取俯仰角度的异构传感融合方案，视觉采集单元仅需选用低成本的单目相机即可，无需配备价格高昂的深度相机或数据手套，角度传感单元可选用百元级的mems传感器或千元级的datalite传感器，整体硬件成本仅为深度相机方案的1/5至1/10，同时角度传感单元直接测量手腕俯仰角的物理量，相比单目视觉通过图像面积变化推算深度的方案，数据稳定性提升80%以上，从根本上避免了光照、遮挡、手部姿态变化带来的深度数据抖动；

57、针对现有技术中离散手势控制导致轨迹阶跃不连续的痛点，本发明首先通过手部骨架关键点提取算法直接获取手腕的连续位置变化，而非识别有限的离散手势类别，从源头上保证了位置输入的连续性，其次通过一阶低通滤波算法滤除输入信号的随机噪声，再通过工业控制器执行单元的固定节拍锁存机制，避免了视觉采集帧率通常为25-30帧/秒与工业控制器扫描频率通常为1000帧/秒以上不匹配带来的指令跳变问题，最后通过在逆运动学求解过程中以上一周期合法关节角为初始参考值，避免了三轴并联机器人逆解多分支切换带来的运动突变，三重机制协同作用下，机器人末端轨迹的平滑度提升90%以上，完全满足精密示教的应用需求，且上述效果在将手部骨架关键点提取算法替换为openpose、hrnet，将角度传感单元替换为mems惯性测量单元，将工业控制器执行单元替换为西门子、倍福等其他品牌的实时控制器时均能稳定实现。