一种通过FCPressNURBS指令集实现复杂曲面的恒力跟踪方法技术

本发明专利技术提供一种通过FCPressNURBS指令集实现复杂曲面的恒力跟踪方法，属于机器人控制技术领域。本发明专利技术包括如下步骤：S10、通过NURBS的轨迹规划方法获取机器人的复杂轨迹规划；S20、获得基于T型速度插补后的NURBS拟合轨迹X

【技术实现步骤摘要】
一种通过FCPressNURBS指令集实现复杂曲面的恒力跟踪方法


[0001]本专利技术专利属于机器人控制
，涉及一种通过FCPressNURBS指令集实现复杂曲面的恒力跟踪方法。

技术介绍

[0002]在机器人接触作业时，需要确定所接触曲面的表面轮廓，并通过安装在机器人末端的多维力传感器感知机器人末端接触力。对于未知环境中的复杂曲面，拟合轨迹的精度影响机器人接触作业时的轨迹跟踪的准确性，而机器人的恒力跟踪算法更是直接影响到机器人末端的恒力跟踪精度。
[0003]目前有不少的文献提出了对曲面的恒力跟踪策略，但大部分算法都只能对较为简单的曲面进行恒力跟踪，且跟踪的精度较低，无法满足现有的工业场景中复杂曲面的恒力跟踪需求；具体的说，主要存在以下几个问题：
[0004]1)传统的轨迹拟合方法只能拟合较为简单的形状，如直线、圆弧或多段线等；
[0005]2)传统的NURBS轨迹规划方法只能拟合机器人的运动轨迹，而无法控制机器人的运动速度；
[0006]3)机器人接触型作业时，不仅需要控制机器人的运动轨迹和运动速度，还需要控制机器人与物体之间的接触力，现有技术中，机器人末端与环境之间的接触力不稳定，难以更好的实现恒力跟踪。

技术实现思路

[0007]本专利技术的目的在于：针对以上问题，本专利技术对传统的单一位置控制或单一恒力控制算法进行改进，提出一种通过FCPressNURBS指令集实现复杂曲面的恒力跟踪方法，通过速度可控NURBS轨迹规划算法拟合复杂环境中的复杂曲面轮廓，并基于阻抗控制算法确保恒力跟踪精度和稳定性，进而确保接触作业时机器人末端轨迹的精度、速度的可控性和接触力的稳定性。
[0008]
技术实现思路
：一种通过FCPressNURBS指令集实现复杂曲面的恒力跟踪方法，包括如下步骤：
[0009]S10、通过NURBS的轨迹规划方法获取机器人的复杂轨迹规划：
[0010]采集曲面上的空间位姿作为FCPressNURBS指令集的型值点，通过型值点获取机器人运动的轨迹规划拟合曲线，并建立拟合曲线参数u与弧长参数s之间的u
‑
s模型；
[0011]S20、基于T型速度插补算法模型对计算得到的u
‑
s模型进行采样，并通过得到的弧长L对曲线相邻的u
i
和u
i+1
之间进行速度插补，获得基于T型速度插补后的NURBS拟合轨迹X
nurbs
；其中，u
i
和u
i+1
是指节点矢量的第i和第i+1个元素；
[0012]S30、使用Squad姿态插补算法获取机器人的姿态，将获取的型值点处的机器人姿态矩阵转换成对应的四元数表示形式然后计算对应离散点的四元数姿态，完成机器人的姿态插补；
[0013]S40、通过阻抗控制算法确定机器人末端与复杂曲面的接触力，利用阻抗控制的输出位移量实时纠正机器人末端运动轨迹，从而保证机器人末端的接触力能够保持在设定的状态，实现恒力跟踪接触复杂曲面。
[0014]进一步的，所述S10包括以下具体步骤：
[0015]S1001、采集多组环境中复杂曲面的空间位姿，作为FCPressNURBS指令集的型值点，通过型值点运动反算控制点算法可以得到拟合轨迹的控制点；
[0016]采集的型值点数量N≥5，型值点的坐标用表示；控制点的坐标用表示；通过追赶法求出所有的控制点；
[0017]S1002、通过控制点获取满足G2连续的三次B样条位置轨迹规划拟合曲线；所述轨迹规划拟合曲线用k次B样条曲线方程表示：
[0018][0019][0020]其中，称为k次B样条基函数，k＝3，i＝0,1,2,
…
,n；P
i
表示控制点，i＝0,1,2,
…
,n；U＝[u0,u1,
…
,u
n+k+1
]表示节点矢量，u∈[0,1]；
[0021]S1003、对所得的k次B样条曲线方程c(u)等参数取样，取样周期为1/n，得到离散后的
ꢀꢀ
表示离散后的节点矢量，表示所对应的c(u)的值；
[0022]S1004、利用布尔公式计算两个参数区间[a,b]的弧长，布尔公式为：
[0023][0024]其中，f(x)用|c
′
(u)|代入计算，x0＝a，x4＝b，x2＝x1+h，x3＝x2+h，f
i
＝f(x
i
)，i＝0,1,
…
,4；
[0025]通过布尔公式可以获得每段弧长，即相邻两离散点之间对应的弧长总弧长从而可以得到弧长参数为
[0026][0027]S1005、使用五次多项式拟合，生成参数u与弧长参数s之间的u
‑
s模型；
[0028]u＝k0+k1s+k2s2+k3s3+k4s4+k5s5[0029]其中，u表示S1003步骤中离散后的s表示弧长参数
[0030]通过最小二乘法求出
[0031]K＝(X
T
X)
‑1X
T
Y
[0032]其中，
[0033]进一步的，所述S20包括以下具体步骤：
[0034]S2001、采用T型速度插补算法，通过总弧长L和基于总弧长设定的加速度以及最大速度可以分别计算出加速段、匀速段和减速段所用时间：
[0035][0036][0037]其中，v0表示初始速度，v1表示最终速度；q0＝s0＝0，q1＝s1＝1；表示加速度，T是机器人的控制周期；
[0038]S2002、结合速度规划算法，分别计算加速段、匀速段和减速段的位移量，不同段的位移计算方程为：
[0039][0040]其中，S
a
、S
v
和S
d
分别表示加速、匀速和减速段的位移量。
[0041]进一步的，所述S30包括以下具体步骤：
[0042]S3001、通过所确定的弧长参数s
i
和离散点u
i
确定四元数球面线性插值算法的t值：
[0043][0044]S3002、通过球面线性插值算法计算多段平滑姿态变化：
[0045]Squad(q
i
,s
i
,s
i+1
,q
i+1
；t)＝Slerp(Slerp(q
i
,q
i+1
；t),Slerp(s
i
,s
i+1
；t)；2t(1
‑
t))。
[0046]进一步的，所述S40包括以下具体步骤：
[0047]S4001、通过阻抗控制算法推导适用于离散点的控制方程，得到基于力控制的机器人位移修正量X
c
：
[0048][0049]本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种通过FCPressNURBS指令集实现复杂曲面的恒力跟踪方法，其特征在于，包括如下步骤：S10、通过NURBS的轨迹规划方法获取机器人的复杂轨迹规划：采集曲面上的空间位姿作为FCPressNURBS指令集的型值点，通过型值点获取机器人运动的轨迹规划拟合曲线，并建立拟合曲线参数u与弧长参数s之间的u
‑
s模型；S20、基于T型速度插补算法模型对计算得到的u
‑
s模型进行采样，并通过得到的弧长L对曲线相邻的u
i
和u
i+1
之间进行速度插补，获得基于T型速度插补后的NURBS拟合轨迹X
nurbs
；其中，u
i
和u
i+1
是指节点矢量的第i和第i+1个元素；S30、使用Squad姿态插补算法获取机器人的姿态，将获取的型值点处的机器人姿态矩阵转换成对应的四元数表示形式然后计算对应离散点的四元数姿态，完成机器人的姿态插补；S40、通过阻抗控制算法确定机器人末端与复杂曲面的接触力，利用阻抗控制的输出位移量实时纠正机器人末端运动轨迹，从而保证机器人末端的接触力能够保持在设定的状态，实现恒力跟踪接触复杂曲面。2.根据要求1所述的一种通过FCPressNURBS指令集实现复杂曲面的恒力跟踪方法，其特征在于，所述S10包括以下具体步骤：S1001、采集多组环境中复杂曲面的空间位姿，作为FCPressNURBS指令集的型值点，通过型值点运动反算控制点算法可以得到拟合轨迹的控制点；采集的型值点数量N≥5，型值点的坐标用表示；控制点的坐标用表示；通过追赶法求出所有的控制点；S1002、通过控制点获取满足G2连续的三次B样条位置轨迹规划拟合曲线；所述轨迹规划拟合曲线用k次B样条曲线方程表示：划拟合曲线用k次B样条曲线方程表示：其中，称为k次B样条基函数，k＝3，i＝0，1，2，
…
，n；P
i
表示控制点，i＝0，1，2，
…
，n；U＝[u0，u1，
…
，u
n+k+1
]表示节点矢量，u∈[0，1]；S1003、对所得的k次B样条曲线方程c(u)等参数取样，取样周期为1/n，得到离散后的和和表示离散后的节点矢量，表示所对应的c(u)的值；S1004、利用布尔公式计算两个参数区间[a，b]的弧长，布尔公式为：
其中，f(x)用|c
′
(u)|代入计算，x0＝a，x4＝b，x2＝x1+h，x3＝x2+h，f
i
＝f(x
i
)，i＝0，1，
…
，4；通过布尔公式可以获得每段弧长，即相邻两离散点之间对应的弧长总弧长从而可以得到弧长参数为S1005、使用五次多项式拟合，生成参数u与弧长参数s之间的u
‑
s模型；u＝k0+k1s+k2s2+k3s3+k4s4+k5s5其中，u表示S1003步骤中离散后的s表示弧长参数通过最小二乘法求出K＝(X
T
X)
‑1X
T
Y其中，...

【专利技术属性】
技术研发人员：段晋军，崔坤坤，姜锦程，郭安，孙伟栋，王灵禺，
申请(专利权)人：南京航空航天大学，
类型：发明
国别省市：