一种基于标架化空间曲线的运动规划方法

摘要

本发明涉及超冗余机器人运动规划技术领域，解决了现有超冗余机器人空间运动步态计算量大，进而导致超冗余机器人运动规划不实时的技术问题，尤其涉及一种基于标架化空间曲线的运动规划方法，该方法包括以下步骤：S1、构建空间参数曲线C(t)作为超冗余机器人的背脊曲线，并计算背脊曲线起始处的单位切向量、单位法向量及单位副法向量构成空间参数曲线初始处的坐标系。本发明通过计算最小旋转标架进行离散化，避免了曲率挠率的计算，对背脊曲线的形状和种类没有要求，对于超冗机器人背脊曲线离散化避免了大量的曲率挠率及积分计算，可以提高计算效率。

权利要求

1.一种基于标架化空间曲线的运动规划方法，其特征在于，该方法包括以下步骤：S1、构建空间参数曲线C(t)作为超冗余机器人的背脊曲线，并计算背脊曲线起始处的单位切向量 、单位法向量 及单位副法向量 构成空间参数曲线初始处的坐标系；

S2、根据空间参数曲线初始处的坐标系 计算前 个连杆长度 对应弧长处的单位切向量 、单位法向量 及单位副法向量 构成最小旋转标架 ；

在步骤S2中，具体过程包括以下步骤：

S21、设已知曲线上距离起始处弧长 的点 处的最小旋转标架为 和曲线上弧长 的点 处的单位切向量 ；

S22、计算最小旋转标架 的单位切向量 基于平面 为镜像的对称向量，其中平面 为点 和点 的二等分面；

S23、将最小旋转标架 中的单位副法向量 基于平面 镜像为对称向量，再将对称向量 基于平面 镜像对称并计算出单位副法向量 ，其中平面为点 处的对称向量 和单位切向量 的二等分面；

S24、采用右手螺旋法则基于单位切向量 和单位副法向量 计算出单位法向量；

S25、根据单位切向量 、单位副法向量 和单位法向量 构成最小旋转标架；

S3、基于第 个最小旋转标架 以及第 个最小旋转标架 生成超冗余机器人的运动规划。

2.根据权利要求1所述的运动规划方法，其特征在于：在步骤S1中，具体过程包括以下步骤：S11、将空间参数曲线 的曲线参数t进行离散化，其中间隔为 ， ，则曲线参数t的取值为 ；

S12、根据曲线参数t和间隔 计算背脊曲线的弧长 ；

S13、令弧长 等于超冗余机器人前 个连杆长度 ，并计算与超冗余机器人前 个连杆长度 相对应的曲线参数 ；

S14、计算曲线参数 处的切向量 ；

S15、计算背脊曲线起始 处的单位切向量 、单位法向量 及单位副法向量。

3.根据权利要求2所述的运动规划方法，其特征在于：在步骤S12中，弧长 的计算公式为：；

上式中，n指曲线段弧长离散求和间隔数，m表示离散曲线段序号数， 表示间隔，表示空间参数曲线C(t)在 处对t的导数， 指向量取模运算。

4.根据权利要求2所述的运动规划方法，其特征在于：在步骤S13中，具体包括：通过以间隔 为积分间隔循环累加离散曲线段的弧长直至弧长 等于前 个连杆长度 ，进而确定满足条件的曲线参数 。

5.根据权利要求2所述的运动规划方法，其特征在于：在步骤S14中，曲线参数 处的切向量 的计算公式为：；

式中， 为空间参数曲线C(t)对曲线参数 的1阶导数。

6.根据权利要求1所述的运动规划方法，其特征在于：在步骤S22中，对称向量 的计算公式为：；

上式中， 指以 为起点 为终点的位置向量， 为单位切向量 在曲线上点 处关于平面 的对称向量。

7.根据权利要求1所述的运动规划方法，其特征在于：在步骤S23中，单位副法向量的计算公式为：；

上式中， 为 关于平面 镜像的对称向量， 为单位切向量 和对称向量 的向量差， 为构成最小旋转标架 姿态矩阵的单位副法向量。

8.根据权利要求1所述的运动规划方法，其特征在于：在步骤S3中，具体过程包括以下步骤：S31、计算第 个最小旋转标架 以及第 个最小旋转标架 的姿态差值；

S32、基于姿态差值 计算超冗余机器人正交关节的关节角 ；

S33、根据关节角 生成超冗余机器人的运动规划。

说明书

一种基于标架化空间曲线的运动规划方法

技术领域

[0001] 本发明涉及超冗余机器人运动规划技术领域，尤其涉及一种基于标架化空间曲线的运动规划方法。

背景技术

[0002] 超冗余机器人由于含有较多的运动冗余度，在狭窄空间探测等领域应用广泛。目前超冗余机器人多采用背脊曲线离散化方法进行运动进行规划。目前采用的基于曲线离散化方法采用曲率和挠率积分的方式计算关节角，该方式需要先计算出背脊曲线方程的曲率和挠率，对于复杂的曲线，计算量非常大，会大大降低离散化效率。

发明内容

[0003] 针对现有技术的不足，本发明提供了一种基于标架化空间曲线的运动规划方法，解决了现有超冗余机器人空间运动规划计算量大，进而导致超冗余机器人运动规划不实时的技术问题。

[0004] 为解决上述技术问题，本发明提供了如下技术方案：一种基于标架化空间曲线的运动规划方法，该方法包括以下步骤：

[0005] S1、构建空间参数曲线C(t)作为超冗余机器人的背脊曲线，并计算背脊曲线起始处的单位切向量 、单位法向量 及单位副法向量 构成空间参数曲线初始处的最小旋转标架的坐标系 ；

[0006] S2、根据空间参数曲线初始处的坐标系 计算前 个连杆长度 对应弧长处的单位切向量 、单位法向量 及单位副法向量 构成最小旋转标架 ；

[0007] S3、基于第 个最小旋转标架 以及第 个最小旋转标架 生成超冗余机器人的运动规划。

[0008] 进一步地，在步骤S1中，具体过程包括以下步骤：

[0009] S11、将空间参数曲线C(t)的曲线参数t进行离散化，其中间隔为 ， ，则曲线参数t的取值为 ；

[0010] S12、根据曲线参数t和间隔 计算背脊曲线的弧长 ；

[0011] S13、令弧长 等于超冗余机器人前 个连杆长度 ，并计算与超冗余机器人前个连杆长度 相对应的曲线参数 ；

[0012] S14、计算曲线参数 处的切向量 ；

[0013] S15、计算背脊曲线起始 处的单位切向量 、单位法向量 及单位副法向量 。

[0014] 进一步地，在步骤S12中，弧长 的计算公式为：

[0015] ；

[0016] 上式中，n指曲线段弧长离散求和间隔数，m表示离散曲线段序号数， 表示间隔，表示空间参数曲线C(t)在 处对t的导数，指向量取模运算。

[0017] 进一步地，在步骤S13中，具体包括：

[0018] 通过以间隔 为积分间隔循环累加离散曲线段的弧长直至弧长 等于前 个连杆长度 ，进而确定满足条件的曲线参数 。

[0019] 进一步地，在步骤S14中，曲线参数 处的切向量 的计算公式为：

[0020] ；

[0021] 上式中， 为空间参数曲线C(t)对曲线参数 的1阶导数。

[0022] 进一步地，在步骤S2中，具体过程包括以下步骤：

[0023] S21、设已知曲线上距离起始处弧长 的点 处的最小旋转标架为 和曲线上弧长 的点 处的单位切向量 ；

[0024] S22、计算最小旋转标架 的单位切向量 基于平面 为镜像的对称向量 ，其中平面 为点 和点 的二等分面；

[0025] S23、将最小旋转标架 中的单位副法向量 基于平面 镜像为对称向量 ，再将对称向量 基于平面 镜像对称并计算出单位副法向量 ，其中平面为点 处的对称向量 和单位切向量 的二等分面；

[0026] S24、采用右手螺旋法则基于单位切向量 和单位副法向量 计算出单位法向量 ；

[0027] S25、根据单位切向量 、单位副法向量 和单位法向量 构成最小旋转标架 。

[0028] 进一步地，在步骤S22中，对称向量 的计算公式为：

[0029] ；

[0030] 上式中， 指以 为起点 为终点的位置向量， 为单位切向量 在曲线上点 处关于平面 的对称向量。

[0031] 进一步地，在步骤S23中，单位副法向量 的计算公式为：

[0032] ；

[0033] 上式中， 为 关于平面 镜像的对称向量， 为单位切向量 和对称向量 的向量差， 为构成最小旋转标架 姿态矩阵的单位副法向量。

[0034] 进一步地，在步骤S3中，具体过程包括以下步骤：

[0035] S31、计算第 个最小旋转标架 以及第 个最小旋转标架 的姿态差值 ；

[0036] S32、基于姿态差值 计算超冗余机器人正交关节的关节角 ；

[0037] S33、根据关节角 生成超冗余机器人的运动规划。

[0038] 借由上述技术方案，本发明提供了一种基于标架化空间曲线的运动规划方法，至少具备以下有益效果：

[0039] 本发明通过计算最小旋转标架进行离散化，避免了曲率挠率的计算，对背脊曲线的形状和种类没有要求，对于超冗余机器人背脊曲线离散化避免了大量的曲率挠率及积分计算，可以提高计算效率。

附图说明

[0040] 此处所说明的附图用来提供对本申请的进一步理解，构成本申请的一部分，本申请的示意性实施例及其说明用于解释本申请，并不构成对本申请的不当限定。在附图中：

[0041] 图1为本发明背脊曲线基于最小旋转标架离散化的示意图；

[0042] 图2为本发明基于空间几何的双反射方法计算最小旋转标架的示意图；

[0043] 图3为本发明生成超冗余机器人运动规划的示意图。

[0044] 图中：1、超冗余机器人连杆；2、最小旋转标架；3、背脊曲线。

具体实施方式

[0045] 为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图 和具体实施方式对本发明作进一步详细的说明。借此对本申请如何应用技术手段来解决技术问题并达成技术功效的实现过程能充分理解并据以实施。

[0046] 本领域普通技术人员可以理解实现实施例方法中的全部或部分步骤是可以通过程序来指令相关的硬件来完成，因此，本申请可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本申请可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD‑ROM、光学存储器等)上实施的计算机程序产品的形式。

[0047] 请参照图1‑图3，示出了本实施例的一种具体实施方式，本实施例通过计算最小旋转标架进行离散化，避免了曲率挠率的计算，对背脊曲线的形状和种类没有要求，对于超冗机器人背脊曲线离散化避免了大量的曲率挠率及积分计算，可以提高计算效率。

[0048] 请参照图1，本实施例提出了一种基于标架化空间曲线的运动规划方法，该方法包括以下步骤：

[0049] S1、构建空间参数曲线C(t)作为超冗余机器人的背脊曲线，并计算背脊曲线起始处的单位切向量 、单位法向量 及单位副法向量 构成空间参数曲线初始处的坐标系 ；

[0050] ；

[0051] 在本实施例中，步骤S1的具体过程包括以下步骤：

[0052] S11、将空间参数曲线C(t)的曲线参数t进行离散化，其中间隔为 ， ，则曲线参数t的取值为 ；

[0053] S12、根据曲线参数t和间隔 计算背脊曲线的弧长 ；

[0054] 弧长 的计算公式为：

[0055] ；

[0056] 上式中，n指曲线段弧长离散求和间隔数，m表示离散曲线段序号数， 表示间隔，表示空间参数曲线C(t)在 处对t的导数， 指向量取模运算。

[0057] S13、令弧长 等于超冗余机器人前 个连杆长度 ，并计算与超冗余机器人前个连杆长度 相对应的曲线参数 ；

[0058] 通过以间隔 为积分间隔循环累加离散曲线段的弧长直至弧长 等于前 个连杆长度 ，进而确定满足条件的曲线参数 ；

[0059] ；

[0060] 上式中， 为空间参数曲线C(t)对曲线参数t的1阶导数，表示进行长度累加求和的连杆数目，表示第i个连杆的长度，表示导数求导符号。

[0061] S14、计算曲线参数 处的切向量 ；

[0062] 曲线参数 处的切向量 的计算公式为：

[0063] ；

[0064] 上式中， 为空间参数曲线C(t)对曲线参数 的1阶导数。

[0065] S15、计算背脊曲线起始 处的单位切向量 、单位法向量 及单位副法向量 ，从而构成空间参数曲线初始处的坐标系 。

[0066] 单位切向量 的计算公式为：

[0067] ；

[0068] 上式中， 为 处的空间参数曲线C(t)对曲线参数t的一阶导数。

[0069] 单位副法向量 的计算公式为：

[0070] ；

[0071] 上式中， 为 处的空间参数曲线C(t)对曲线参数t的二阶导数，表示向量叉乘运算。

[0072] 单位法向量 的计算公式为：

[0073] ；

[0074] 上式中， 为背脊曲线起始处的单位副法向量， 为背脊曲线起始处的单位切向量。

[0075] S2、根据空间参数曲线初始处的坐标系 计算前 个连杆长度 对应弧长处，即曲线上点 处的单位切向量 、单位法向量 及单位副法向量 构成最小旋转标架。

[0076] 在本实施例中，步骤S2的具体过程包括以下步骤：

[0077] S21、设已知曲线上距离起始处弧长 的点 处的最小旋转标架为 和曲线上弧长 的点 处的单位切向量 ；

[0078] S22、计算最小旋转标架 的单位切向量 基于平面 为镜像的对称向量 ，其中平面 为点 和点 的二等分面；

[0079] S23、将最小旋转标架 中的单位副法向量 基于平面 镜像为对称向量 ，再将对称向量 基于平面 镜像对称并计算出单位副法向量 ，其中平面为点 处的对称向量 和单位切向量 的二等分面；

[0080] S24、采用右手螺旋法则基于单位切向量 和单位副法向量 计算出单位法向量 ；

[0081] S25、根据单位切向量 、单位副法向量 和单位法向量 构成最小旋转标架 。

[0082] 请参照图2，基于空间几何的双反射方法计算最小旋转标架 的过程如下：

[0083] 首先假设已知曲线上点 处的最小旋转标架为 ；

[0084] ；

[0085] 上式中， 为曲线上点 处的单位切向量， 为曲线上点 处的单位法向量， 为曲线上点 处的单位副法向量。

[0086] 为确定起点 处最小旋转标架 ，则可以计算最小旋转标架 的单位切向量 基于平面 为镜像的对称向量 ，其中平面 为点 和点 的二等分面，则有：

[0087] ；

[0088] 上式中， 指以 为起点 为终点的位置向量， 为单位切向量 在曲线上点 处关于平面 的对称向量。

[0089] 同理将最小旋转标架 中的单位副法向量 基于平面 镜像为对称向量，再将对称向量 基于平面 镜像对称并计算出单位副法向量 。

[0090] 其中平面 为点 处的对称向量 和单位切向量 的二等分面， 为曲线在 处的单位切向量，通过公式（5）可以计算，关于单位副法向量 计算公式如下：

[0091] ；

[0092] 上式中， 为 关于平面 镜像的对称向量， 为单位切向量 和对称向量 的向量差， 为构成最小旋转标架 姿态矩阵的单位副法向量。

[0093] 最后采用右手螺旋法则基于单位切向量 和单位副法向量 计算出单位法向量 ，单位法向量 的计算公式为：

[0094] ；

[0095] 其中 为构成最小旋转标架 姿态矩阵 的单位法向量，进一步构成最小旋转标架 。

[0096] S3、基于第 个最小旋转标架 以及第 个最小旋转标架 生成超冗余机器人的运动规划；

[0097] 具体的，第 个最小旋转标架 的表达式为：

[0098] ；

[0099] 第 个最小旋转标架 的表达式为：

[0100] ；

[0101] 在本实施例中，步骤S3的具体过程包括以下步骤：

[0102] S31、计算第 个最小旋转标架 以及第 个最小旋转标架 的姿态差值 ；

[0103] 具体的，姿态差值 的计算公式为：

[0104] ；

[0105] 上式中， 。

[0106] S32、基于姿态差值 计算超冗余机器人正交关节的关节角 ；

[0107] 具体的，关节角 的计算公式为：

[0108] ；

[0109] 其中，由于超冗余机器人关节为正交关节，奇数关节进行侧摆运动，偶数关节进行俯仰运动，因此奇数关节角和偶数关节角的计算方式不同，通过该步骤计算出关节角 ，进而生成超冗余机器人的运动规划。

[0110] S33、根据关节角 生成超冗余机器人的运动规划。

[0111] 请参照图3，曲线1缓慢变化到曲线2及曲线3，基于最小旋转标架 通过公式（18）计算出随着时间变化的相应的关节角 ，完成超冗余机器人的运动规划，图3中X、Y、Z分别为描述背脊曲线的坐标系的三维坐标方向。

[0112] 作为示例，图3中曲线1、曲线2和曲线3均为空间四次贝塞尔曲线，它们的参数方程分别为：

[0113] 曲线1的参数方程为：

[0114] ；

[0115] 曲线2的参数方程为：

[0116] ；

[0117] 曲线3的参数方程为：

[0118] ；

[0119] 其中， ，则它们在背脊曲线起始处的单位切向量 ，

[0120] 单位法向量 ，

[0121] 单位副法向量 ，

[0122] 设定超冗余机器人的连杆长度 为0.1m；

[0123] 因此，示例的背脊曲线根据连杆长度 基于最小旋转标架 进行离散化，则可以计算出曲线1、2、3分别对应的超冗余机器人正交关节的关节角 为：

[0124] 曲线1对应的超冗余机器人正交关节的关节角 为：

[0125] [0.067,‑0.470,0.052,‑0.594,0.060,‑0.389,0.0983,‑0.187,0.182,‑0.083,0.343,‑0.0241,0.471,0.022,0.342,0.079,0.181,0.177,0.097,0.364,0.059,0.577,
0.048,0.490,0.059,0.266,0.086,0.138,0.127,0.077,0.185,0.046]弧度。

[0126] 曲线2对应的超冗余机器人正交关节的关节角 为：

[0127] [0.064,‑0.643,0.046,‑0.730,0.061,‑0.348,0.108,‑0.146,0.190,‑0.063,0.304,‑0.019,0.363,0.012,0.286,0.046,0.182,0.094,0.114,0.173,0.076,0.294,
0.056,0.413,0.051,0.399,0.062,0.273,0.093,0.163,0.159,0.095,0.296,0.056]弧度。

[0128] 曲线3对应的超冗余机器人正交关节的关节角 为：

[0129] [0.064,‑0.646,0.045,‑0.746,0.058,‑0.360,0.096,‑0.155,0.158,‑0.073,0.236,‑0.033,0.290,‑0.008,0.269,0.014,0.200,0.0440,0.138,0.089,0.096,0.168,
0.070,0.303,0.057,0.451,0.060,0.430,0.096,0.270,0.240,0.142]弧度。

[0130] 从而在得到随着时间变化的相应的关节角 后，以此完成对与超冗余机器人的运动规划。

[0131] 本实施例对于超冗机器人背脊曲线离散化避免了大量的曲率挠率及积分计算，可以提高计算效率。

[0132] 本说明书中的各个实施例均采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同或相似的部分互相参见即可。对于以上各实施例而言，由于其与方法实施例基本相似，所以描述的比较简单，相关之处参见方法实施例的部分说明即可。

[0133] 以上实施方式对本发明进行了详细介绍，本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处，综上所述，本说明书内容不应理解为对本发明的限制。