CN102778886B - 平面四自由度机械臂控制***模拟验证平台 - Google Patents

Abstract

本发明公开了一种平面四自由度机械臂控制***模拟验证平台，包括4个模拟机械臂关节，1套六维力传感器，2个臂杆，1个带双目视觉相机的末端执行器，1套远距离测量相机及远距离测量相机控制器，1套中距离测量相机及中距离测量相机控制器，UMAC运动控制卡，工控机，气浮平台和模拟固定墙；本发明可在平面运动状态下，实现大型空间机械臂大负载、多自由度***高精度、高稳定伺服控制算法模拟试验验证、末端执行器抓取、碰撞动力学与控制试验验证及空间机械臂动力学与控制***耦合特性的模拟试验验证，并与仿真模型进行分析迭代，为空间大型机械臂控制***算法和关键技术攻关提供一种验证方法。

Description

平面四自由度机械臂控制***模拟验证平台

技术领域

本发明涉及空间大型机械臂控制***及末端抓捕、释放机构地面模拟验证技术，属于机构技术领域，尤其涉及一种平面四自由度机械臂控制***模拟验证平台。

背景技术

空间机械臂属于空间大型多自由度、大跨度、多体运动***.其地面重力状态下三维空间运动难以实现，一些控制***关键技术：比如多自由度***高精度、高稳定伺服控制，抓取碰撞问题，基于动力学的控制算法设计与验证，动力学与控制***耦合特性等问题难以进行地面验证。国际空间站机械臂***(SSRMS)、国际空间站欧洲臂(ERA)均采用仿真手段，但***仿真距离实际应用还存在较大差距。

发明内容

本发明提供了平面四自由度机械臂控制***模拟验证平台，对空间大型机械臂控制***及末端抓捕、释放技术进行充分的地面试验验证。

本发明的目的是通过下述技术方案实现的：

该平台包括4个模拟机械臂关节即第一关节、第二关节、第三关节、第四关节，1套六维力传感器，模拟机械臂杆A，模拟机械臂杆B，1套带双目视觉相机的末端执行器，1套远距离测量相机及远距离测量相机控制器，1套中距离测量相机及中距离测量相机控制器，UMAC运动控制卡，工控机，模拟舱壁目标标志器及目标适配器，气浮平台和模拟固定墙；

4个模拟机械臂关节和两根臂杆搭建成平面四自由度模拟机械臂：将第二关节、第三关节分别垂直安装在第二气浮架上，第一关节垂直放置.第四关节水平安装在第三关节上且与第三关节垂直，第一关节与第二关节通过模拟机械臂杆A连接，第二关节与第三关节通过模拟机械臂杆B连接；

平面四自由度模拟机械臂的第一关节通过连接件固定在模拟固定墙上，第四关节一端连接带双目视觉相机的末端执行器，所述末端执行器放置在第一气浮架上；第四关节和末端执行器之间安装六维力传感器；远距离测量相机放置在模拟固定墙平行位置且调整到能观测到目标适配器，中距离测量相机放置在模拟机械臂杆B且调整到能观测到目标适配器，工控机通过以太网与远距离测量相机控制器、中距离测量相机控制器、UMAC运动控制卡相连；UMAC运动控制卡与模拟机械臂关节相连；模拟舱壁目标标志器及目标适配器放置在平面四自由度模拟机械臂能够抓捕到的范围以内；以上组成平面四自由度机械臂控制***模拟验证平台，将该平台安装在气浮平台上；

工控机中装有***控制及调试仿真单元，具体包括路径规划模块、动力学计算模块、力／位混合控制模块、数据通信及显示模块、图像可视化模块、末端执行器控制模块；

该机械臂控制***模拟验证的具体步骤为：

1)路径规划模块接收各相机对模拟舱壁目标标志器视觉位姿测量信息、UMAC运动控制卡控制的各关节的角度和速度信息，工控机设定的机械臂末端的运动曲线和周期时间；

在一个周期时间内，路径规划模块根据测量的机械臂的DH参数求出雅可比矩阵，然后利用模拟舱壁目标标志器的视觉位姿测量信息和雅可比矩阵的伪逆.求出各关节的运动角度，进而求出各关节速度和加速度信息发送给动力学计算模块；各数据信息同步在数据通信及显示模块中进行显示，各相机的图像在图像可视化模块中进行显示；

各相机的视觉测量信息具体如下：

远距离测量相机实现对模拟舱壁目标标志器的空间位姿测量.测显范围大于10m；

中距离测量相机完成对模拟舱壁目标标志器进行三维位姿测量，测量范围为10m～1.3m；

双目视觉相机能在可视范围内对周边环境进行清晰成像，包括舱壁目标标志器接口状况、舱壁目标标志器、周边设备及障碍物.测量范围为1.3m以内；

2)动力学计算模块根据各关节运动角度、速度和加速度信息计算出各个关节各个时刻的质量矩阵，并将各关节运动角度、速度和加速度和各个关节各个时刻的质量矩阵发送给UMAC运动控制卡；

3)UMAC运动控制卡根据各关节运动角度、速度、加速度和各个关节各个时刻的质量矩阵，调用关节伺服控制算法控制各关节运动；

4)一个周期时间完成后，进入下一个周期.循环执行1)至3)直至机械臂末端进入设定的捕获范围时，通过工控机对末端执行器控制模块发送捕获指令，末端执行器完成捕获后执行第5)步：

5)当末端执行器与目标适配器接触后，六维力传感器由自身解算电路对六维力传感器力和力矩信息进行解耦，并将解耦后的六维力和力矩信息上传至力／位混合控制模块，力／位混合控制模块将六维力／力矩转换为末端的位姿信息，并传输给路径规划模块，实现末端执行器拖动过程中机械臂末端位姿调整.从而实现末端执行器的拖动和锁紧。

臂杆长度、直径可根据真实机械臂基频进行确定。

远距离测量相机的高度为3m.中距离测量相机的高度为0.51m。

本发明的有益效果：

可在平面运动状态下，实现大型空间机械臂大负载、多自由度***高精度、高稳定伺服控制算法验证、末端执行器抓取、碰撞动力学与控制试验验证及空间机械臂动力学与控制***耦合特性的验证，并与仿真模型进行分析迭代.为空间大型机械臂控制***算法和关键技术攻关提供一种验证方法。

附图说明

1.图1为***控制及仿真软件框图；

2.图2为该验证平台流程图；

3.图3为平面四自由度模拟机械臂示意图；

4.图4为该模拟验证平台示意图；

1-第一关节、2-第二关节、3-第三关节、4-第四关节、5-末端执行器、6-第一气浮架、7-模拟固定墙、8-第二气浮架、9-远距离测量相机、10-中距离测量相机、11-模拟机械臂杆A、12-气浮平台、13-目标适配器，14-模拟机械臂杆B：具体实施方式

为了更好地理解本发明的技术方案，下面结合附图及具体实施例对本发明做进一步详细描述。

本发明提供了一种平面四自由度机械臂控制***模拟验证平台，根据真实的空间机械臂产品，设计与真实产品具有相同的动力学特性平面运动模拟机械臂，安装在气浮平台上，验证机械臂控制策略、机械臂控制***调试方法及碰撞力最小的末端抓捕，释放控制方法，研究***动力学控制***特性，并达到与仿真模型进行分析迭代的目的。

该平台包括4个模拟机械臂关节即第一关节1、第二关节2、第三关节3、第四关节4，1套六维力传感器，模拟机械臂杆A11，模拟机械臂杆B14，1套带双目视觉相机的末端执行器5，1套远距离测量相机9及远距离测量相机控制器.1套中距离测量相机10及中距离测量相机控制器，UMAC运动控制卡，工控机，气浮平台12、模拟舱壁目标标志器及目标适配器13和模拟固定墙7；其中末端执行器采用申请号为“200910122845.7”，发明名称为《一种绳索捕获式末端执行器》中所述末端执行器；

4个模拟机械臂关节和两根臂杆搭建成平面四自由度模拟机械臂：将第二关节2、第三关节3分别垂直安装在第二气浮架8上，第一关节1垂直放置，第四关节4水平安装在第三关节3上且与第三关节3垂直，第一关节1与第二关节2通过模拟机械臂杆A11连接，第二关节2与第三关节3通过模拟机械臂杆B14连接；

平面四自由度模拟机械臂的第一关节1通过连接件固定在模拟固定墙7上，第四关节4一端连接带双目视觉相机的末端执行器.所述末端执行器放置在第一气浮架6上；第四关节和末端执行器之间安装六维力传感器；远距离测量相机9放置在模拟固定墙7平行位置且调整到能观测到目标适配器13，中距离测量相机10放置在模拟机械臂杆B14且调整到能观测到目标适配器13，工控机通过以太网与远距离测量相机控制器、中距离测量相机控制器、UMAC运动控制卡相连；UMAC运动控制卡与模拟机械臂关节相连：模拟舱壁目标标志器及目标适配器13放置在平面四自由度模拟机械臂能够抓捕到的范围以内；以上组成平面四自由度机械臂控制***模拟验证平台，将该平台安装在气浮平台12上：

工控机中装有***控制及调试仿真单元，具体包括路径规划模块、动力学计算模块、力／位混合控制模块、数据通信及显示模块、图像可视化模块、末端执行器控制模块；

该机械臂控制***模拟验证的具体步骤为：

1)路径规划模块接收各相机的视觉位姿测量信息、UMAC运动控制卡控制的各关节的角度和速度信息；调用路径规划算法，产生各关节运动角度、速度和加速度信息发送给动力学计算模块；各数据信息同步在数据通信及显示模块中进行显示，各相机的图像在图像可视化模块中进行显示；

其中基于视觉测量的机械臂末端路径规划为：

以模拟固定墙与机械臂连接点为原点，与平台短边相平行的方向为X轴，与平台长边相平行的方向为Y轴，与平台相垂直的方向为Z轴，建立空间坐标系；假定机械臂初始位姿为X_e0=[P_e0，ψ_e0]，视觉相机测量的目标位姿分别记为：X_ef=[P_ef，ψ_ef]，并假定机械臂末端进行沿X_e0到X_ef的直线路径运动，则起点和终点分别为P_e0(x₀.y₀.z₀.α₀.β₀.γ₀)和P_ef(x_f.y_f.z_f.α_f.βf_.γ_f).x₀.y₀.z ₀.为起点在空间坐标系中的位置，α0.β₀.γ₀为起点在空间坐标系的姿态，x_f.y_f.z_f.为终点在空间坐标系中的位置，α_f.β_f.γf为终点在空间坐标系的姿态；运行时间t人为设定运行时间包括：加速时间t_A，匀速运行时间t_S和减速时间t_S；在运行时间内.每个周期计算时间为T；

根据机械臂构型，可确定机械臂DH参数，然后，采用矢量积方法，建立机械臂的雅可比矩阵，因为雅可比矩阵的每一列向量代表相应的关节速度对机械臂末端线速度和角速度的影响，所以求雅可比矩阵可以分别求出它的每一列，，本文所设计机械臂具有4个自由度，所以求出雅可比矩阵为4×6矩阵。

根据雅可比逆矩阵，每个周期关节速度为：

$\stackrel{.}{\mathrm{\θ}}=J^{-}\×{\stackrel{.}{X.}}_{\mathrm{ef}}$

其中，为关节速度向量，

为机械臂末端速度向量，J^-为雅可比矩阵逆矩阵。

当前时刻关节角度向量为：

${\mathrm{\θ}}_{n+1}={\mathrm{\θ}}_n+\stackrel{.}{\mathrm{\θ}}\×T$

其中，θ_n为当前关节角度向量，θ_n+1为下一时刻关节角度向量，T为规划周期。

各相机的视觉测量信息具体如下：

首先远距离测量相机能对在可视范围内空间环境实现清晰成像，包括机械臂、目标舱段及其粗定位标靶，远距离测量相机控制器实现对粗定位标靶的位姿测量，并将图像和位姿测量信息实时地上传给***控制及调试仿真软件；全局相机测量模拟舱壁目标标志器的空间位姿.引导机械臂向目标方向移动.其测量范围大于10m；

其次，安装于四自由度模拟机械臂臂杆上，当中距离测量相机能在可视范围内对周边环境进行清晰成像时，由中距离测量相机完成对模拟舱壁目标标志器进行三维位姿测量，若机械臂初始阶段姿态调整较好，中距离测量相机可以直接观测到模拟舱壁目标标志器，否则需要它利用其云台扩展器其视场范围，重新调整机械臂姿态后再对模拟舱壁目标标志器进行测量；它能计算出目标的三维姿态，并传送给***控制及调试仿真单元.引导机械臂接近目标.其测量范围为10m～1.3m；

最后，双目视觉相机能在可视范围内对周边环境进行清晰成像，包括模拟舱壁目标标志器接口状况、舱壁目标标志器、周边设备及障碍物等.并能把图像和位姿测量信息实时地上传给***控制及调试仿真单元，其清晰成像的范围为1.3m以内；

2)动力学计算模块根据各关节运动角度、速度和加速度信息计算出各个关节各个时刻的质量矩阵，并将各关节运动角度、速度和加速度和各个关节各个时刻的质量矩阵发送给UMAC运动控制卡；

3)UMAC运动控制卡根据各关节运动角度、速度、加速度和各个关节各个时刻的质量矩阵，调用关节伺服控制算法控制各模拟关节运动；

4)当机械臂末端进入捕获范围时，通过工控机对末端执行器控制模块发送捕获指令，末端执行器完成捕获后执行第5)步；反之则循环执行1)至3)；

5)当末端执行器与目标适配器13接触后，六维力传感器由自身解算电路对六维力传感器力和力矩信息进行解耦.并将解耦后的六维力和力矩信息上传至力／位混合控制模块转换为末端的位姿信息传输给路径规划模块，实现末端执行器拖动过程中机械臂末端位姿调整，人为发送指令给末端执行器控制模块，从而实现碰撞力最小的末端抓捕或释放控制。力和力矩信息上与术端的位姿信息为线性关系。

其中，1、2、3关节为偏航运动关节，4关节为回转运动关节，四个一体化关节、两根臂杆及六维力传感器搭建成平面四自由度模拟机械臂，以模拟大型空间机械臂在气浮平台上的平面运动；模拟机械臂末端连接一套末端执行器，实现对具有目标适配器接口的目标物的捕获、抓取和释放；第四关节和末端执行器之间安装六维力传感器，测量末端执行器抓捕过程中末端受力，作为***力控制的输入；

该仿真单元进一步包括初始化模块和控制***调试模块，以实现初始化功能和控制***调试功能。验证过程中，所有相关参数均同步上传于仿真单元，可根据模拟验证平台数据对仿真模型进行修正。

综上所述，以上仅为本发明的较佳实施例而已，并非用于限定本发明的保护范围。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (3)

1.机械臂控制***模拟验证平台，其特征在于，包括：4个模拟机械臂关节即第一关节、第二关节、第三关节、第四关节，1套六维力传感器，模拟机械臂杆A，模拟机械臂杆B，1套带双目视觉相机的末端执行器，1套远距离测量相机及远距离测量相机控制器，1套中距离测量相机及中距离测量相机控制器，UMAC运动控制卡，工控机，模拟舱壁目标标志器及目标适配器，气浮平台和模拟固定墙；

4个模拟机械臂关节和两根臂杆搭建成平面四自由度模拟机械臂：将第二关节、第三关节分别垂直安装在第二气浮架上，第一关节垂直放置，第四关节水平安装在第三关节上且与第三关节垂直，第一关节与第二关节通过模拟机械臂杆A连接，第二关节与第三关节通过模拟机械臂杆B连接；

平面四自由度模拟机械臂的第一关节通过连接件固定在模拟固定墙上，第四关节一端连接带双目视觉相机的末端执行器，所述末端执行器放置在第一气浮架上；第四关节和末端执行器之间安装六维力传感器；远距离测量相机放置在模拟固定墙平行位置且调整到能观测到目标适配器，中距离测量相机放置在模拟机械臂杆B且调整到能观测到目标适配器，工控机通过以太网与远距离测量相机控制器、中距离测量相机控制器、UMAC运动控制卡相连；UMAC运动控制卡与模拟机械臂关节相连；模拟舱壁目标标志器及目标适配器放置在平面四自由度模拟机械臂能够抓捕到的范围以内；以上组成平面四自由度机械臂控制***模拟验证平台，将该平台安装在气浮平台上；

工控机中装有***控制及调试仿真单元，具体包括路径规划模块、动力学计算模块、力/位混合控制模块、数据通信及显示模块、图像可视化模块、末端执行器控制模块；

该机械臂控制***模拟验证的具体步骤为：

1）路径规划模块接收各相机对模拟舱壁目标标志器视觉位姿测量信息、UMAC运动控制卡控制的各关节的角度和速度信息，工控机设定的机械臂末端的运动曲线和周期时间；

在一个周期时间内，路径规划模块根据测量的机械臂的直径和长度参数求出雅可比矩阵，然后利用模拟舱壁目标标志器的视觉位姿测量信息和雅可比矩阵的伪逆，求出各关节的运动角度，进而求出各关节速度和加速度信息发送给动力学计算模块；各数据信息同步在数据通信及显示模块中进行显示，各相机的图像在图像可视化模块中进行显示；

各相机的视觉测量信息具体如下：

远距离测量相机实现对模拟舱壁目标标志器的空间位姿测量，测量范围大于10m；

中距离测量相机完成对模拟舱壁目标标志器进行三维位姿测量，测量范围为10m～1.3m；

双目视觉相机能在可视范围内对周边环境进行清晰成像，包括舱壁目标标志器接口状况、舱壁目标标志器、周边设备及障碍物，测量范围为1.3m以内；

2）动力学计算模块根据各关节运动角度、速度和加速度信息计算出各个关节各个时刻的质量矩阵，并将各关节运动角度、速度和加速度和各个关节各个时刻的质量矩阵发送给UMAC运动控制卡；

3）UMAC运动控制卡根据各关节运动角度、速度、加速度和各个关节各个时刻的质量矩阵，调用关节伺服控制算法控制各关节运动；

4）一个周期时间完成后，进入下一个周期，循环执行1）至3）直至机械臂末端进入设定的捕获范围时，通过工控机对末端执行器控制模块发送捕获指令，末端执行器完成捕获后执行第5）步；

5）当末端执行器与目标适配器接触后，六维力传感器由自身解算电路对六维力传感器力和力矩信息进行解耦，并将解耦后的六维力和力矩信息上传至力/位混合控制模块，力/位混合控制模块将六维力／力矩转换为末端的位姿信息，并传输给路径规划模块，实现末端执行器拖动过程中机械臂末端位姿调整，从而实现末端执行器的拖动和锁紧。

2.如权利要求1所述的机械臂控制***模拟验证平台，其特征在于，臂杆长度、直径可根据真实机械臂基频进行确定。

3.如权利要求1所述的机械臂控制***模拟验证平台，其特征在于，远距离测量相机的高度为3m，中距离测量相机的高度为0.51m。

Images