CN114310915B - 基于视觉反馈的空间机械臂对接末端工具轨迹规划方法 - Google Patents

Abstract

基于视觉反馈的空间机械臂对接末端工具轨迹规划方法，涉及轨迹规划领域。本发明是为了解决目前的机械臂对接末端工具的方法还存在对接精度低、发射成本高的问题。本发明包括：获取机械臂在工具箱标志器上方的视觉测量准备位置时机械臂末端工具系与工具箱标志器坐标系之间的相对位姿[d γ]；将[d γ]修正至预设的标称值；利用预设的标称值规划机械臂笛卡尔空间运动轨迹获得机械臂末端路径对应的关节空间轨迹；引入机械臂末端x方向的受力信息，将按照机械臂笛卡尔空间运动轨迹进行运动后的机械臂末端工具系作为规划初始末端工具系规划机械臂运动，获得空间机械臂对接末端工具的轨迹。本发明用于规划空间机械臂对接末端工具的运动轨迹。

Description

基于视觉反馈的空间机械臂对接末端工具轨迹规划方法

技术领域

本发明涉及轨迹规划领域，特别涉及基于视觉反馈的空间机械臂对接末端工具轨迹规划方法。

背景技术

空间机器人需要携带多种工具完成复杂的在轨任务，工具通常放置在服务飞行器的工具箱中，空间机器人需要自主完成工具取放操作。然而由于发射过程中引起的机械结构震动变形以及在轨环境高低温变化导致的热变形，导致机械臂与工具箱的安装位置实际值与模型名义值产生偏差，因此如何克服这些偏差从而准确取放工具成为本领域的研究重点。

目前本领域主要采用增大工具对接口的方式增大容差的方式来避免对偏差进行补偿，进而实现空间机械臂和末端工具的对接，这种方式虽然完成了空间机械臂和末端工具的对接，但是会导致对接的精度降低，且增大工具对接口不仅会导致加工成本变大，也会导致发射重量变重，从而增加了发射成本。

发明内容

本发明目的是为了解决目前的机械臂对接末端工具的方法还存在对接精度低、发射成本高的问题，而提出了基于视觉反馈的空间机械臂对接末端工具轨迹规划方法。

基于视觉反馈的空间机械臂对接末端工具轨迹规划方法具体过程为：

步骤一、获取机械臂在工具箱标志器上方的视觉测量准备位置时机械臂末端工具系与工具箱标志器坐标系之间的相对位姿[d γ]；

其中，d是机械臂末端工具系与工具箱标志器坐标系之间的相对距离，γ是机械臂末端工具系与工具箱标志器坐标系之间的相对姿态；

步骤二、将步骤一获得的[d γ]修正至预设的标称值[d_rx d_ry d_rz γ_rx γ_ryγ_rz]；

其中，d_rx、d_ry、d_rz分别依次是工具箱标志器与标称末端工具系标称x、y、z方向的距离；γ_rx、γ_ry、γ_rz分别依次是工具箱标志器与标称末端工具系x、y、z轴的欧拉角角度；

步骤三、利用预设的标称值[d_rx d_ry d_rz γ_rx γ_ry γ_rz]规划机械臂笛卡尔空间运动轨迹获得机械臂末端路径对应的关节空间轨迹

使得标称末端工具系与工具箱标志器坐标系y、z方向的相对位置和相对姿态收敛至0，实现机械臂末端快换与工具接口对准；

步骤四、引入机械臂末端x方向的受力信息，将按照步骤三获得的关节空间轨迹

进行运动后的机械臂末端工具系作为规划初始末端工具系规划机械臂运动，获得机械臂对接末端工具的关节轨迹

即为空间机械臂对接末端工具的轨迹。

本发明的有益效果为：

本发明引入了视觉反馈信息，修正了由于机械臂与工具箱的安装位置偏差而引起的相对位姿误差。且本发明在对接过程的最后阶段引入了机械臂末端受力信息修正期望位置轨迹，保证轨迹精度的同时，控制了对接过程中机械臂与工具之间的碰撞力，实现安全的软对接。本发明通过提升对接精度的方式提升了对接准确率，避免了由于增大工具对接口而产生的加工成本，同时由于没有增大工具对接口，减轻了发射的重量，降低了发射成本。

附图说明

图1为机械臂关节坐标系；

图2为对接过程中主要坐标系；

图3为对接末端工具方法流程图；

图4为对接过程中主要构型示意图；

其中，图4(a)为初始位置构型示意图；图4(b)为视觉测量准备位置构型示意图；图4(c)为标称末端工具系位置构型示意图；图4(d)为对准工具位置构型示意图；图4(e)为工具对接完成位置构型示意图；

图5为机械臂由初始位置运动到视觉测量工具箱标志器的准备位置的关节角度曲线；

图6为机械臂将末端工具系位置修正到标称末端工具系位置对应的相机测量数据曲线和关节角度变化曲线图；

其中，图6(a)为相机位置测量数据变化曲线图；图6(b)为相机姿态测量数据变化曲线图；图6(c)为关节角度变化数据变化曲线图；

图7为机械臂由标称末端工具系位置到对接目标过程中关节角度和X方向末端受力变化曲线图；

其中，图7(a)为关节角度变化曲线图；图7(b)为对接过程末端x方向受力变化曲线图。

具体实施方式

具体实施方式一：本实施方式基于视觉反馈的空间机械臂对接末端工具轨迹规划方法具体过程如图3所示，包括以下步骤：

步骤一、利用安装在机械臂末端的手眼相机获取机械臂在工具箱标志器上方的视觉测量准备位置时(图4(b))机械臂末端工具系与工具箱标志器坐标系之间的相对位姿[dγ]；

其中，d是机械臂末端工具系与工具箱标志器坐标系之间的相对距离，γ是机械臂末端工具系与工具箱标志器坐标系之间的相对姿态；

所述机械臂为7自由度机械臂，机械臂关节坐标系如图1所示，对接过程中用到的主要坐标系如图2所示；

步骤二、将步骤一获得的[d γ]修正至预设的标称值[d_rx d_ry d_rz γ_rx γ_ryγ_rz]，相机测量数据变化曲线和关节角度变化曲线如图6所示；

其中，d_rx、d_ry、d_rz分别是工具箱标志器与标称末端工具系(图4(c))x、y、z方向的距离；γ_rx、γ_ry、γ_rz分别工具箱标志器与标称末端工具系x、y、z轴的欧拉角角度；

步骤三、利用预设的标称值[d_rx d_ry d_rz γ_rx γ_ry γ_rz]规划机械臂笛卡尔空间运动轨迹，使得标称末端工具系与工具箱标志器坐标系y、z方向的相对位置和相对姿态收敛至0，实现机械臂末端快换与工具接口对准(图4(d))；

步骤四、引入机械臂末端x方向的受力信息，将按照步骤三获得的关节空间轨迹

进行运动后的机械臂位置作为规划初始位置规划机械臂运动，获得机械臂对接末端工具的关节轨迹即为空间机械臂对接末端工具的轨迹，按照空间机械臂对接末端工具的轨迹完成对接(图4(e))，关节角度变化和x方向受力变化如图7所示。

具体实施方式二：所述步骤一中的机械臂在工具箱标志器上方的视觉测量准备位置，为从机械臂初始位置(图4(a))按照关节空间运动轨迹运动获得，关节角度曲线如图5所示；

所述关节空间运动轨迹通过3次样条插值规划获得，具体如下：

其中，θ_d，

和

为规划的机械臂关节位置，速度和加速度，j∈[1,n]，n为预定义的轨迹点数量，p₁,o₂,…p_n为n个轨迹点，每个轨迹点p_j有8个元素，前7个元素为7个关节位置，第8个元素为到达该位置的时刻，cubicspline()是三次样条插值函数。

具体实施方式三：所述步骤一中的机械臂在工具箱标志器上方的视觉测量准备位置时机械臂末端工具系与工具箱标志器坐标系之间的相对位姿[d γ]通过以下方式获得：

由于手眼相机存在测量噪声，采用多次测量取平均的方法抵消噪声影响，测量次数取为100。最终获得的机械臂末端工具系与工具箱标志器坐标系的相对位姿为：

其中，d_i，γ_i为第i次手眼相机测量得到的相对位置和相对姿态(ZYX欧拉角表示)，d和γ为平均后的机械臂末端工具系与工具箱标志器坐标系之间的相对位置和姿态。

具体实施方式四：所述步骤二中的将步骤一获得的[d γ]修正至预设的标称值[d_rx d_ry d_rz γ_rx γ_ry γ_rz]后机械臂在标称末端工具系下的关节空间运动轨迹如下：

步骤二一、利用步骤一获得的[d γ]获取工具箱标志器坐标系相对于机械臂末端工具系的位姿矩阵为：

^eT_v＝wz2mtrx(dγ)

式中，上标e表示末端工具系，下标v表示工具箱标志器坐标系，wz2mtrx()是位姿信息至位姿矩阵的运算函数；

步骤二二、利用预设的标称值获取工具箱标志器坐标系相对于机械臂标称末端工具系的位姿矩阵如下：

^mT_v＝wz2mtrx([d_rx d_ry d_rz γ_rx γ_ry γ_rz])

式中，m表示机械臂标称末端工具系；

步骤二三、根据机械臂关节位置传感器反馈的关节角度信息获取机械臂基座系相对于机械臂末端工具系的位姿矩阵^bT_e，机械臂的关节坐标系如图1所示，对应的DH参数表如表1所示；

^bT_e＝fdkine(θ，l，^bT₀)

其中，θ为7个关节角度的矢量，l为机械臂连杆长度参数即为连杆偏距d_i的值，^bT₀为常值矩阵，

^bT_e表示由末端工具系至机械臂基座系的位姿矩阵，b表示机械臂基座系，fdkine()是正运动学求解函数；

表1 机械臂DH参数表

步骤二四、利用步骤二一到步骤二三获得的位姿矩阵获取机械臂在标称末端工具系下的关节角度θ_m：

步骤二四一、利用步骤二一到步骤二三获得的位姿矩阵获取机械臂标称末端工具系相对于机械臂基座系的位姿矩阵：

^bT_m＝^bT_e ^eT_v(^mT_v)^-1

步骤二四二、基于锁定关节2逆运动学求解步骤二四一获得的^bT_m对应的关节角度：

θ_m＝ikine(^bT_m，θ₂)

式中，θ₂为指定的关节2角度，ikine()是逆运动学求解函数；

步骤二五、利用步骤二四获得的θ_m按照3次样条插值规划关节空间的运动轨迹获得获得机械臂在标称末端工具系下的关节空间运动轨迹，如下：

式中，p_init＝[θ_init 0]为第1个轨迹点，θ_init为规划初始的当前关节位置，p_end＝[θ_mt_m]为第2个轨迹点，t_m为设定的到达标称工具系位置的时刻；

步骤二六、机械臂按照步骤二五获得的

运动，到达标称末端工具系位置,该位置末端工具系与工具箱标志器坐标系的相对位姿[d_rx d_ry d_rz γ_rx γ_ryγ_rz]；

具体实施方式五、所述步骤三中利用预设的标称值[d_rx d_ry d_rz γ_rx γ_ry γ_rz]规划机械臂笛卡尔空间运动轨迹，使得标称末端工具系与工具箱标志器坐标系Y、Z方向的相对位置和相对姿态收敛至0，实现机械臂末端快换与工具接口对准，包括以下步骤：

步骤三一、根据标称末端工具系下的机械臂的关节角度获取标称末端工具系相对于机械臂基座系的位姿矩阵^bT_e,start：

^bT_e，start＝fdkine(θ_start，l，^bT₀)

其中，θ_start是标称末端工具系下的机械臂的关节角度；

步骤三二、设定规划完成时间t_f，对元素d_ry，d_rz，γ_rx，γ_ry和γ_rz进行规划，获得规划结果：

式中，t是0时刻到当前时刻经历的时间，0≤t≤t_f，k＝1～5，x_init,1至x_init,5分别依次对应d_ry、d_rz、γ_rx、γ_ry、γ_rz；

步骤三三、利用步骤三一获得的^bT_e,start和步骤三二获得的x_k对机械臂末端工具系的期望路径进行规划获得规划后的机械臂末端工具系的期望路径，如下：

其中，rotz(·)、roty(·)和rotx(·)分别表示绕z轴、y轴、x轴旋转的单位位姿矩阵；

步骤三四、对步骤三三获得的机械臂末端工具系的期望路径进行求解获得机械臂末端路径对应的关节空间轨迹：

其中，q_d2＝ikine(^bT_r,θ₂)是基于锁定关节2逆运动学求解位姿矩阵对应的关节角度，interpolator()是插补函数，ikine()是逆运动学求解函数；

机械臂按照θ_d2，

运动，实现机械臂末端快换与工具接口对准。

具体实施方式六：所述步骤四中的引入机械臂末端X方向的受力信息，将按照步骤三获得的关节空间轨迹

进行运动后的机械臂位置作为规划初始位置规划机械臂运动，获得机械臂对接末端工具的关节轨迹即为空间机械臂对接末端工具的轨迹，包括以下步骤：

步骤四一、将按照步骤三获得的关节空间轨迹

进行运动后的机械臂末端工具系作为规划初始末端工具系，关节角度作为初始关节角度θ_1start，获取规划初始末端工具系相对于机械臂基座系的位姿矩阵：

^bT’_e，start＝fdkine(θ₁s_tart，l，^bT₀)

步骤四二、根据关节力矩传感器信息，利用步骤四一获得的^bT’_e,start获取规划初始末端工具系下机械臂末端x方向的受力f_ext,x：

f_ext，x＝F_ext[4]

其中，

^bR_e，start＝^bT’_e，start(1∶3，1∶3)

其中，^bR_e，start为^bT’_e，start前三行和前三列元素，J为机械臂Jacobian矩阵，J^T表示机械臂Jacobian矩阵的转置，τ_ext为关节力矩传感器测量得到的空间机械臂关节力矩矢量，F_ext为计算得到的规划初始末端工具系下表示的机械臂末端受力，它的第4个元素为末端x方向受力；

步骤四三、设定对接距离为d_f＝d_rx+0.1，为了保证克服对接的阻力，实现可靠连接，相比于标称x方向的距离d_rx，最终规划的对接距离增加了0.1m，规划初始末端工具系下的机械臂末端x方向的运动轨迹为：

其中，s_dx、

依次是规划初始末端工具系下机械臂末端在x方向的运动距离、运动速度、运动加速度；

步骤四四、利用步骤四一获得的^bT’_e，start获取规划初始末端工具系下的机械臂末端位置和末端速度：

步骤四四一、利用步骤四一获得的^bT’_e，start获取末端工具系相对于规划初始末端工具系的位姿矩阵为：

^e，startT_e＝(^bT’_e，start)^{-1 b}T_e

步骤四四二、利用步骤四四一获得的^e，startT_e获取规划初始末端工具系x方向实际前进的距离：

s_ex＝^e，startT_e(1，4)

其中，s_ex为矩阵^e，startT_e第1行第4列的元素；

步骤四四三、利用步骤四二获得的^bR_e，start获取规划初始末端工具系下x方向的速度矢量：

式中，

为初始末端工具系下x方向的速度矢量，为初始末端工具系下的末端速度矢量v的第4个元素；

其中，

式中，

为关节角速度矢量。

步骤四五、根据步骤四二获得的f_ext,x和步骤四四获得的s_ex和

修正步骤四三获得的规划初始末端工具系下的机械臂末端X方向的运动轨迹，修正后的期望轨迹如下：

其中，

式中，s_rx，

为经过末端受力信息修正过的期望位置、速度和加速度，m_imp，b_imp和k_imp为期望惯量、阻尼和刚度参数；

步骤四六、利用步骤四一获得的^bT’_e,start和步骤四五获得的s_rx获取机械臂末端工具系的期望路径：

步骤四七、利用步骤四六获得的^bT’_r获取机械臂对接末端工具的关节轨迹：

基于锁定关节2逆运动学求解位姿矩阵对应的关节角度：

q_d3＝ikine(^bT′_r，θ₂)

对q_d3进行二次多项式插补，得到细分的关节轨迹：

其中，interpolator()是插补函数。

机械臂按照θ_d3，

运动，保证轨迹精度的同时，控制对接过程中机械臂与工具之间的碰撞力，实现安全的软对接。

实施例：

为本发明的有效性，使用计算机仿真。搭建计算机仿真平台，仿真过程如下：

[步骤S1]机械臂初始关节角度为[-166.5，-125，209.3，-95.83，-11.98，105.98，-79.41]deg，按照如下7个预编程轨迹点进行三次样条插值规划，运动到工具箱标志器上方的视觉测量准备位置；

p₁＝[-160.50 -125.00 209.30 -95.83 -11.98 105.98 -79.41 0]

p₂＝[-159.70 -122.00 200.70 -89.82 -9.80 107.10 -82.33 10]

p₃＝[-158.50 -122.00 201.90 -94.40 -5.71 108.10 -82.12 15]

p₄＝[-157.20 -122.00 202.90 -98.81 -1.48 109.20 -81.86 20]

p₅＝[-155.60 -122.00 203.60 -103.00 2.87 110.50 -81.55 25]

p₆＝[-153.90 -122.00 204.20 -107.00 7.39 111.90 -81.17 30]

p₇＝[-151.90 -122.00 204.50 -110.80 12.11 113.50 -80.70 35]

[步骤S2]机械臂保持当前位置，安装在机械臂末端的手眼相机测量机械臂末端工具系与工具箱标志器坐标系之间的相对位姿，获得100个测量结果后，取均值，最终相对位姿结果为[d γ]＝[550.5mm，209.8mm，-0.4mm，0.02deg，-0.04deg，0.02deg]；

[步骤S3]规划机械臂运动，修正机械臂末端工具系与工具系标志器坐标系之间的相对位姿至标称值d_rx＝450mm，d_ry＝180mm，d_rz＝60mm，γ_rx＝0deg，γ_ry＝0deg，γ_rz＝0deg，修正至标称值时对应的关节角度为[-152.53，-122，204.64，-115.42，16.19，113.03，-80.84]deg：

[步骤S4]规划机械臂在笛卡尔空间的运动轨迹，使得末端工具系与工具箱标志器坐标系Y、Z方向的相对位置和相对姿态收敛至0，规划初始的关节角度为[-152.53，-122，204.64，-115.42，16.19，113.03，-80.84]deg，在40s(t_f＝40)内在初始末端工具系下，沿y方向运动180mm，沿z方向运动60mm，保持姿态不变，实现机械臂末端快换与工具接口对准，对准后机械臂末端工具系与工具箱标志器坐标系的相对位姿为[450mm，0mm，0mm，0deg，0deg，0deg]；

[步骤S5]规划机械臂在笛卡尔空间的运动轨迹，实现机械臂软对接工具，规划初始的关节角度为[-152.53，-122，204.64，-115.42，16.19，113.03，-80.84]deg，在40s(t_f＝40)内在初始末端工具系下，沿末端x方向运动550mm(d_f＝450+100)，设定阻抗参数为m_imp＝50kg，b_imp＝1000Ns/m，k＝400N/m，引入末端受力信息修正规划的期望轨迹，实现机械臂与目标的软对接。

Claims (9)

1.基于视觉反馈的空间机械臂对接末端工具轨迹规划方法，其特征在于所述方法具体过程为：

步骤一、获取机械臂在工具箱标志器上方的视觉测量准备位置时机械臂末端工具系与工具箱标志器坐标系之间的相对位姿[d γ]；

其中，d是机械臂末端工具系与工具箱标志器坐标系之间的相对距离，γ是机械臂末端工具系与工具箱标志器坐标系之间的相对姿态；

步骤二、将步骤一获得的[d γ]修正至预设的标称值[d_rx d_ry d_rz γ_rx γ_ry γ_rz]；

其中，d_rx、d_ry、d_rz分别依次是工具箱标志器与标称末端工具系标称x、y、z方向的距离；γ_rx、γ_ry、γ_rz分别依次是工具箱标志器与标称末端工具系x、y、z轴的欧拉角角度；

所述将步骤一获得的[d γ]修正至预设的标称值[d_rx d_ry d_rz γ_rx γ_ry γ_rz]后机械臂在标称末端工具系下的关节空间运动轨迹

通过以下方式获得：

步骤二一、利用步骤一获得的[d γ]获取工具箱标志器坐标系相对于机械臂末端工具系的位姿矩阵^eT_v：

^eT_v＝wz2mtrx(d γ)

式中，e是机械臂末端工具系，v是工具箱标志器坐标系，wz2mtrx()是位姿信息至位姿矩阵的运算函数；

步骤二二、利用预设的标称值获取工具箱标志器坐标系相对于机械臂标称末端工具系的位姿矩阵^mT_v：

^mT_v＝wz2mtrx([d_rx d_ry d_rz γ_rx γ_ry γ_rz])

式中，m表示机械臂标称末端工具系；

步骤二三、根据机械臂关节位置传感器反馈的关节角度信息获取机械臂基座系相对于机械臂末端工具系的位姿矩阵^bT_e：

^bT_e＝fdkine(θ，l，^bT₀)

其中，θ为7个关节角度的矢量，l为机械臂连杆长度参数，

b表示机械臂基座系，fdkine()是正运动学求解函数；

步骤二四、利用步骤二一到步骤二三获得的位姿矩阵获取机械臂在标称末端工具系下的关节角度θ_m；

步骤二五、利用步骤二四获得的θ_m按照3次样条插值规划关节空间的运动轨迹获得机械臂在标称末端工具系下的关节空间运动轨迹

步骤三、利用预设的标称值[d_rx d_ry d_rz γ_rx γ_ry γ_rz]规划机械臂笛卡尔空间运动轨迹获得机械臂末端路径对应的关节空间轨迹

使得标称末端工具系与工具箱标志器坐标系y、z方向的相对位置和相对姿态收敛至0，实现机械臂末端快换与工具接口对准；步骤四、引入机械臂末端x方向的受力信息，将按照步骤三获得的关节空间轨迹

进行运动后的机械臂末端工具系作为规划初始末端工具系规划机械臂运动，获得机械臂对接末端工具的关节轨迹

即为空间机械臂对接末端工具的轨迹。

2.根据权利要求1所述的基于视觉反馈的空间机械臂对接末端工具轨迹规划方法，其特征在于：所述步骤一中的机械臂在工具箱标志器上方的视觉测量准备位置为机械臂按照关节空间运动轨迹运动获得的，所述关节空间运动轨迹通过如下方式规划获得：

其中，θ_d，

和

分别依次为规划获得的机械臂关节位置，速度和加速度，n为预定义的轨迹点数量，p₁，p₂，...p_n为n个轨迹点，每个轨迹点p_j有8个元素，j∈[1，n]，前7个元素为7个关节位置，第8个元素为到达工具箱标志器上方的视觉测量准备位置的时刻，cubicspline()是三次样条插值函数。

3.根据权利要求2所述的基于视觉反馈的空间机械臂对接末端工具轨迹规划方法，其特征在于：所述步骤一中机械臂在工具箱标志器上方的视觉测量准备位置时机械臂末端工具系与工具箱标志器坐标系之间的相对位姿[d γ]如下：

其中，d_i，γ_i为第i次测量得到的相对位置和相对姿态，C为测量总次数，i∈[1，C]，d和γ依次为C次测量后平均得到的机械臂末端工具系与工具箱标志器坐标系之间的相对位置和姿态。

4.根据权利要求3所述的基于视觉反馈的空间机械臂对接末端工具轨迹规划方法，其特征在于：所述步骤二四中的利用步骤二一到步骤二三获得的位姿矩阵获取机械臂在标称末端工具系下的关节角度θ_m，包括以下步骤：

步骤二四一、利用步骤二一到步骤二三获得的位姿矩阵获取机械臂标称末端工具系相对于机械臂基座系的位姿矩阵：

^bT_m＝^bT_e ^eT_v(^mT_v)^-1

步骤二四二、基于锁定关节a逆运动学求解步骤二四一获得的^bT_m对应的关节角度即机械臂在标称末端工具系下的关节角度θ_m：

θ_m＝ikine(^bT_m，θ_a)

式中，θ_a为指定关节a的角度，ikine()是逆运动学求解函数，a取{1，2，3，5，6，7}。

5.根据权利要求4所述的基于视觉反馈的空间机械臂对接末端工具轨迹规划方法，其特征在于：所述步骤二五中利用步骤二四获得的θ_m按照3次样条插值规划关节空间的运动轨迹获得机械臂在标称末端工具系下的关节空间运动轨迹

具体如下：

式中，p_init＝[θ_intt 0]为第1个轨迹点，p_end＝[θ_m t_m]为第2个轨迹点，t_m为设定的到达标称工具系位置的时刻，θ_init为规划初始的当前关节位置。

6.根据权利要求5所述的基于视觉反馈的空间机械臂对接末端工具轨迹规划方法，其特征在于：所述步骤三中利用预设的标称值[d_rx d_ry d_rz γ_rx γ_ry γ_rz]规划机械臂笛卡尔空间运动轨迹获得机械臂末端路径对应的关节空间轨迹

包括以下步骤：

步骤三一、根据标称末端工具系下的机械臂的关节角度获取标称末端工具系相对于机械臂基座系的位姿矩阵^bT_e，start：

^bT_e，start＝fdkine(θ_start，l，^bT₀)

其中，θ_start是标称末端工具系下的机械臂的关节角度；

步骤三二、设定规划完成时间t_f，对元素d_ry，d_rz，γ_rx，γ_ry和γ_rz进行规划，获得规划结果x_k：

式中，t是0时刻到当前时刻经历的时间，0≤t≤t_f，k＝1～5，x_init，1至x_init，5分别依次对应d_ry、d_rz、γ_rx、γ_ry、γ_rz；

步骤三三、利用步骤三一获得的^bT_e，start和步骤三二获得的x_k对机械臂末端工具系的期望路径进行规划，获得规划后的机械臂末端工具系的期望路径^bT_r，如下：

其中，rotz(·)、roty(·)和rotx(·)分别表示绕z轴、y轴、x轴旋转的单位位姿矩阵，I是单位矩阵；

步骤三四、对步骤三三获得的机械臂末端工具系的期望路径^bT_r进行求解获得机械臂末端路径对应的关节空间轨迹

其中，q_d2＝ikine(^bT_r，θ_a)是基于锁定关节a逆运动学求解位姿矩阵对应的关节角度，interpolator()是插补函数，ikine()是逆运动学求解函数，a取{1，2，3，5，6，7}是关节标号。

7.根据权利要求6所述的基于视觉反馈的空间机械臂对接末端工具轨迹规划方法，其特征在于：所述步骤四中引入机械臂末端x方向的受力信息，将按照步骤三获得的关节空间轨迹

进行运动后的机械臂末端工具系作为规划初始末端工具系规划机械臂运动，获得机械臂对接末端工具的关节轨迹

包括以下步骤：

步骤四一、将按照步骤三获得的关节空间轨迹

进行运动后的机械臂末端工具系作为规划初始末端工具系，关节角度作为初始关节角度θ_1start，利用规划初始末端工具系和θ_1start获取规划初始末端工具系相对于机械臂基座系的位姿矩阵^bT’_e，start：

^bT’_e，start＝fdkine(θ_1st ^rt，l，^bT₀)

步骤四二、利用步骤四一获得的^bT’_e，start获取规划初始末端工具系下机械臂末端x方向的受力f_ext，x：

f_ext，x＝F_ext[4]

其中，

^bR_e，start＝^bT’_e，start(1：3，1：3)

式中，^bR_e，start为^bT’_e，start前三行和前三列元素，J为机械臂Jacobian矩阵，J^T表示机械臂Jacobian矩阵的转置，τ_ext为关节力矩传感器测量得到的空间机械臂关节力矩矢量，F_ext规划初始末端工具系下表示的机械臂末端受力；

步骤四三、设定对接距离d_f，规划初始末端工具系下的机械臂末端x方向的运动轨迹为：

其中，对接距离d_f＝d_rx+L，L为最终规划的对接距离增加的距离，s_dx、

依次是规划初始末端工具系下机械臂末端在x方向的运动距离、运动速度、运动加速度；

步骤四四、利用步骤四一获得的^bT’_e，start获取规划初始末端工具系下的x方向的实际前进距离s_ex和x方向的末端速度矢量

步骤四五、利用步骤四二获得的f_ext，x和步骤四四获得的s_ex和

修正步骤四三获得的规划初始末端工具系下的机械臂末端x方向的运动轨迹，获得修正后的初始末端工具系下的机械臂末端x方向的运动轨迹s_rx：

其中，

式中，s_rx，

依次为经过末端受力信息修正过的期望位置、速度和加速度，m_imp，b_imp和k_imp依次为期望惯量、阻尼和刚度参数；

步骤四六、用步骤四一获得的^bT’_e，start和步骤四五获得的s_rx获取机械臂末端工具系的期望路径^bT’_r：

其中，I是单位矩阵；

步骤四七、利用步骤四六获得的^bT’_r获取机械臂对接末端工具的关节轨迹

8.根据权利要求7所述的基于视觉反馈的空间机械臂对接末端工具轨迹规划方法，其特征在于：所述步骤四四中的利用步骤四一获得的^bT’_e，start获取规划初始末端工具系下的x方向的实际前进距离s_ex和x方向的末端速度矢量

包括以下步骤：

步骤四四一、利用步骤四一获得的^bT’_e，start获取末端工具系相对于规划初始末端工具系的位姿矩阵^e，startT_e：

^e，startT_e＝(^bT’_e，start)^-1bT_e

步骤四四二、利用步骤四四一获得的^e，startT_e获取规划初始末端工具系x方向实际前进的距离s_ex：

s_ex＝_e，star ^tT_e (1，4)

步骤四四三、利用步骤四二获得的^bR_e，start获取规划初始末端工具系下x方向的速度矢量

式中，v[4]为初始末端工具系下的末端速度矢量v的第4个元素；

其中，

式中，

为关节角速度矢量。

9.利要求8述的基于视觉反馈的空间机械臂对接末端工具轨迹规划方法，其特征在于：所述步骤四七中的利用步骤四六获得的^bT’_r获取机械臂对接末端工具的关节轨迹

具有如下：

步骤四七一、利用步骤四六获得的^bT’_r基于锁定关节a逆运动学求解位姿矩阵对应的关节角度：

q_d3＝ikine(^bT″_r，θ_a)

步骤四七二、对q_d3进行二次多项式插补，得到细分的关节轨迹：

其中，interpolator()是插补函数。

Images