CN116237950A - 基于分段运动规划策略的机器人末端精确控制方法及设备 - Google Patents

Abstract

基于分段运动规划策略的机器人末端精确控制方法及设备，属于超冗余机器人运动规划及控制技术领域。为了解决现有超冗余机器人末端运动精度低的问题，本发明将超冗余机器人在运动学链上分成基部，颈部和头部，基部采用背脊曲线进行运动学的设计，并进行离散化计算出基部的关节角；然后计算超冗余机器人头部工作空间，并确定灵活工作空间的中心；通过用头部末端连杆的期望位姿和头部的灵活工作空间得出用于颈部的末端参考坐标系的期望位置和方向，使用最优化算法计算颈部和头部的关节角度。适用于超冗余机器人的末端控制。

Description

基于分段运动规划策略的机器人末端精确控制方法及设备

技术领域

本发明属于超冗余机器人运动规划及控制技术领域，具体涉及一种机器人末端精确控制方法、存储介质及设备。

背景技术

超冗余机器人具有良好的环境适应性和灵活的自由度，在诸多领域具有非常广阔的应用前景。由于超冗余机器人具有冗余的运动自由度，因此运动比较灵活。在非结构环境下运动时经常需要控制机器人末端进行抓取或者勘探操作。但是目前超冗余机器人多采用基于背脊曲线的运动控制方法，这种控制方法存在机器人末端的运动控制精度低，难以实现精确控制等问题，同时该方式需要设计背脊曲线并进行离散化等复杂计算步骤，因此严重影响了超冗余机器人的控制和应用。

发明内容

本发明是为了解决现有超冗余机器人末端运动精度低的问题，提供了一种基于分段运动规划策略的机器人末端精确控制方法。

基于分段运动规划策略的机器人末端精确控制方法，针对超冗余机器人进行末端精确控制，所述超冗余机器人为蛇形机器人，包括头部、颈部和基部，头部和基部通过颈部连接；

针对超冗余机器人进行末端精确控制的过程包括以下步骤：

S1、通过平面圆弧背脊曲线确定超冗余机器人的基部的关节角；

S2、确定灵活工作空间的中心：

采用TRAC-IK算法计算超冗余机器人头部工作空间，计算出的头部工作空间共有N_p个点，以其中的第i个空间点p_i为球心，半径R_p构建球面S_i，i＝1,2,3,…,N_p；在S_i上均匀分布N_s个点其中第k个点为p_i,k，k＝1,2,…,N_s；以

为Z轴方向，以p_i,k为原点构建坐标系W_i,k，计算过程如下：

其中，

分别为W_i，k坐标系Z轴、X轴、Y轴单位向量，W_i，k坐标系原点为p_i,k；v_set为用户自定义三维单位向量；则每个p_i构成的球面上均匀分布Ns个期望位姿

使用TRAC-IK算法对每个求取逆运动学解，假设Ns个姿态能求取m个解；基于工作空间，根据可达度指标D确定灵活工作空间，进而确定出灵活工作空间的中心；

S3、将超冗余机器人头部的灵活工作空间中心与头部末端期望位置重合，确定头部基座原点即颈部末端的期望位置，使用最优化方法使颈部末端实际的位置和末端局部坐标系X轴方向靠近期望的位置和方向，进而解出颈部的关节角：

首先确定颈部末端连杆的期望位置：

头部的基坐标系也是颈部的末端坐标系；设灵活工作空间的中心点、头部的基坐标系原点和基部的末端点分别为G₀、B₀和P₀，则有

L_c＝‖B₀-G₀‖ (5)

P_d＝G₀-L_cr_d (7)

其中，L_c为灵活工作空间的中心点到头部的基坐标系原点的距离；P_d为颈部末端连杆的期望位置；r_d为颈部末端连杆的期望方向；

然后，颈部关节角的求解问题便转化为颈部末端姿态与期望位姿误差最小的最优化问题；假设颈部有n_c个关节，颈部关节的关节角为θ_k,θ_k+1,....,

因此有

其中，^kT_k+1为第k+1关节坐标系到第k个坐标系的齐次变换矩阵；E_n为目标函数；P_a和r_a分别为颈部末端连杆的实际位置和方向；h是权重系数；

和

分别为关节角的关节限位下限和上限；

S4、将颈部的末端作为头部的基座采用TRAC-IK逆运动学算法求解出头部的关节角。

进一步地，通过平面圆弧背脊曲线确定超冗余机器人的基部的关节角的过程包括以下步骤：

假设基部的背脊曲线的长度为l，通过离散化半径为r的圆弧获得基部的关节角；对于基部的背脊曲线，有

其中，θ为关节角度，r为基部圆弧背脊曲线的半径；S_c为基部的背脊曲线围成的接触多边形面积；当θ＝π，S_c有最大值为1/2πr²；

当基部的背脊曲线确定后，通过离散化背脊曲线获得基部关节角。

进一步地，通过离散化背脊曲线获得基部关节角的过程包括以下步骤：

采取基于曲率挠率积分的离散化方法计算基部关节角，基部关节角包括侧摆关节和俯仰关节关节角：

式中，Δs为侧摆关节连接的两个连杆的长度之和；κ(s)、κ_c(s)、κ_w(s)分别为基部平面背脊曲线曲率在弧长s处的曲率、曲率在俯仰平面和侧摆平面的分量；θ_yaw、θ_pitch分别为侧摆关节和俯仰关节的关节角；

带入

得

进一步地，所述可达度指标如下：

其中，m为单个工作空间点构成球面分布的Ns期望位姿能求出解的数目。

进一步地，根据可达度指标D确定灵活工作空间的过程中，选取可达度指标D等于100的工作空间作为灵活工作空间。

进一步地，确定灵活工作空间的中心的过程包括以下步骤：

采用k-means++的方法进行灵活工作空间中心的筛选，进而确定出灵活工作空间的中心。

进一步地，以p_i,k为原点构建坐标系W_i,k的过程中，若计算出的

则将v_set重新设置为[0,1,0]。

进一步地，用户自定义三维单位向量v_set设置为[1,0,0]。

一种计算机存储介质，所述存储介质中存储有至少一条指令，所述至少一条指令由处理器加载并执行以实现所述的基于分段运动规划策略的机器人末端精确控制方法。

一种基于分段运动规划策略的机器人末端精确控制设备，所述设备包括处理器和存储器，所述存储器中存储有至少一条指令，所述至少一条指令由处理器加载并执行以实现所述的基于分段运动规划策略的机器人末端精确控制方法。

本发明由于采取以上技术方案，其具有以下优点：

本发明方法将超冗余机器人在运动学链上进行分段，可以在保持超冗余机器人运动稳定的情况下实现边运动边进行末端的抓取或者侦测操作，可以有效的控制末端位置误差，即本发明的控制具有较高的末端运动控制精度。

附图说明

图1为基于最小旋转坐标系的背脊曲线离散化流程图。

图2为运动学分段及灵活工作空间示意图。

图3为蛇形机器人末端捕捉小球的仿真过程图。

具体实施方式

具体实施方式一：

本实施方式为一种基于分段运动规划策略的机器人末端精确控制方法，包括以下步骤：

步骤一、超冗余机器人为蛇形机器人，包括头部、颈部和基部(即躯干)，头部和基部通过颈部连接；假设基部的背脊曲线的长度为l，通过离散化半径为r的圆弧获得基部的关节角。对于基部的背脊曲线，有

其中，θ为关节角度，r为基部圆弧背脊曲线的半径。S_c为基部的背脊曲线围成的接触多边形面积；当θ＝π，S_c有最大值为1/2πr²。

当基部的背脊曲线确定后，需要离散化背脊曲线获得基部关节角：

采取基于曲率挠率积分的离散化方法(通过公式(3)(4)实现)计算基部关节角，基部关节角包括侧摆关节和俯仰关节关节角，基部的背脊曲线为平面的关节角，公式为

式中，Δs为侧摆关节连接的两个连杆的长度之和。κ(s)、κ_c(s)、κ_w(s)分别为基部平面背脊曲线曲率在弧长s处的曲率、曲率在俯仰平面和侧摆平面的分量；θ_yaw、θ_pitch分别为侧摆关节和俯仰关节的关节角；

带入

得

基部的关节角确定后可以计算出基部的末端坐标系，基部的末端坐标系即为颈部的基座坐标系。

步骤二、采用TRAC-IK算法计算超冗余机器人头部工作空间，计算出的头部工作空间共有N_p个点，以其中的第i个空间点p_i为球心，半径R_p构建球面S_i，i＝1,2,3,…,N_p。在S_i上均匀分布N_s个点其中第k个点为p_i,k，k＝1,2,…,N_s。以

为Z轴方向，以p_i,k为原点构建坐标系W_i,k，计算过程如下：

其中，

分别为W_i，k坐标系Z轴、X轴、Y轴单位向量，W_i，k坐标系原点为p_i,k。v_set为用户自定义三维单位向量，可以设置为[1,0,0]。若以此计算出的

≤1e-5(10的-5次方)，则将v_set重新设置为[0,1,0]。则每个p_i构成的球面上均匀分布Ns个期望位姿

用户自定义三维单位向量可以是任意设置的，理论上都会有可能出现≤1e-5的情况，实际算法实现设置为[1,0,0]比较方便；但是当≤1e-5时必须重新设置为[0,1,0]。

使用TRAC-IK算法对每个求取逆运动学解，假设Ns个姿态能求取m个解。根据超冗余机器人的连杆长度、机器人构型，选取出可达度指标D等于100的工作空间作为灵活工作空间。其中可达度指标计算如下：

其中，m为单个工作空间点构成球面分布的Ns期望位姿能求出解的数目，D为可达度指标。

采用k-means++的方法进行灵活工作空间中心的筛选，进而确定出灵活工作空间的中心。

步骤三、由于颈部连接着头部和基部，因此根据基部和头部确定颈部的关节角。当头部的灵活工作空间覆盖期望的位姿时，则说明该位姿可解。因此确定头部的基坐标系的位姿显得十分重要。本发明中头部的基坐标系也就是颈部的末端坐标系。这里假设灵活工作空间的中心点、头部的基坐标系原点和基部的末端点分别为G₀、B₀和P₀，则有

L_c＝‖B₀-G₀‖ (5)

P_d＝G₀-L_cr_d (7)

其中，L_c为灵活工作空间的中心点到头部的基坐标系原点的距离；P_d为颈部末端连杆的期望位置；r_d为颈部末端连杆的期望方向。G₀、B₀和P₀分别为灵活工作空间的中心点、头部的基坐标系原点和基部的末端点。

图2是运动学分段及灵活工作空间示意图，图中：工作空间1、头部连杆2、颈部连杆3、支撑多边形4、基部连杆5、末端期望位姿6、颈部期望方向位置7。

将超冗余机器人头部的期望位置与灵活工作空间的中心点重合，进而反推出颈部末端的期望位置和期望方向。一旦实际颈部末端坐标系原点(即头部的基坐标系原点)达到或者接近期望的位置且实际颈部末端坐标系X轴方向(末端连杆的轴线方向)达到或者接近期望方向时，则便能保证头部的灵活工作空间可以覆盖头部末端关节的期望位姿。因此颈部关节角的求解问题便转化为颈部末端姿态与期望位姿误差最小的最优化问题。假设颈部有n_c个关节，颈部关节的关节角为θ_k,θ_k+1,....,

因此有

其中，^kT_k+1为第k+1关节坐标系到第k个坐标系的齐次变换矩阵；E_n为目标函数；P_a和r_a分别为颈部末端连杆的实际位置和方向；h是权重系数。θ_i表示蛇形机器人第i个关节角，

和

分别为关节角的关节限位下限和上限。

步骤四、基于灵活工作空间的分段运动学(分段运动学也即步骤一、二、三中的基部、颈部和头部的三段分段，步骤一、二、三即在解释灵活工作空间的分段运动学)包含基部、颈部和头部三个部分逆运动学关节角的求解：

首先通过平面圆弧背脊曲线确定超冗余机器人的基部的关节角。

其次，将超冗余机器人头部的灵活工作空间中心与头部末端期望位置重合，进而通过(5)(6)(7)确定头部基座原点即颈部末端的期望位置，使用最优化方法使颈部末端实际的位置和末端局部坐标系X轴方向(颈部末端轴线)靠近期望的位置和方向，进而解出颈部的关节角。

最后，将颈部的末端作为头部的基座采用TRAC-IK逆运动学算法求解出头部的关节角。

步骤四具体通过下步骤实现：

(1)首先确定基部的末端点P₀。通过离散化基部的背脊曲线来确定蛇形机器人基部的末端点P₀。

(2)计算颈部的末端连杆轴线的期望方向r_d，即从P₀到头部期望位置G₀的方向。

(3)将头部灵活工作空间的中心与头部末端的期望位置重合进而根据头部基座与灵活工作空间中心的距离反推去确定颈部末端坐标系原点的期望位置，参考(5)(6)(7)。

(4)根据将目标函数E_n最小化进而确定颈部的关节角。

(5)将颈部末端的坐标系当作头部的基坐标系，进而可以使用TRAC-IK逆运动学算法确定蛇形机器人的头部关节角。

具体实施方式二：

本实施方式为一种计算机存储介质，所述存储介质中存储有至少一条指令，所述至少一条指令由处理器加载并执行以实现所述的基于分段运动规划策略的机器人末端精确控制方法。

应当理解，指令包括本发明描述的任何方法对应的计算机程序产品、软件或计算机化方法；所述指令可以用于编程计算机系统，或其他电子装置。计算机存储介质可以包括其上存储有指令的可读介质，可以包括但不限于磁存储介质，光存储介质；磁光存储介质包括只读存储器ROM、随机存取存储器RAM、可擦除可编程存储器(例如，EPROM和EEPROM)以及闪存层，或者适合于存储电子指令的其他类型的介质。

具体实施方式三：

本实施方式为一种基于分段运动规划策略的机器人末端精确控制设备，所述设备包括处理器和存储器，应当理解，包括本发明描述的任何包括处理器和存储器的设备，设备还可以包括其他通过信号或指令进行显示、交互、处理、控制等以及其他功能的单元、模块；

所述存储器中存储有至少一条指令，所述至少一条指令由处理器加载并执行以实现所述的基于分段运动规划策略的机器人末端精确控制方法。

实施例1：

通过具体实施方式一能够实现超冗余机器人头部末端的精确位姿求解。接下来介绍基于实际任务场景的应用，如图1所示，本实施例的处理过程包括以下步骤：

(1)首先初始化蛇形机器人头部末端期望姿态的欧拉角α、β和γ。确定末端期望位置为P_s。离散化背脊曲线获得初始状态需要的关节角。初始状态由于对末端背脊曲线末端没做方向约束，背脊曲线可以由平面圆弧和平面三次贝塞尔曲线组成，在连接处光滑连接。通过平面背脊曲线离散化的方法计算蛇形机器人的关节角。

(2)将蛇形机器人从当前状态规划到基部呈半圆状，颈部及头部呈螺旋线外扩状背脊曲线所对应的机器人连杆状态。

(3)通过头部的灵活工作空间和头部末端连杆的期望位姿确定颈部末端的期望位置和方向，进而求解颈部关节角。确定颈部关节角后，基于颈部末端坐标系，采用TRAC-IK算法求解头部关节角。基于整个蛇形机器人构型和关节角，判断ZMP(zero-moment point)稳定性。若求解的ZMP稳定，则规划关节运动。再判断笛卡尔空间运动过程是否ZMP稳定。若迭代次数m大于等于m₀，结束迭代求解。若迭代次数m小于m₀，则进行目标函数

计算，若E小于阈值ε，则结束求解，否则跳回(3)重新执行。m₀设置为1000，h取值为0.2。ε取值1e-4。

(4)若迭代结束无解，则需要采用滚动步态和转弯步态将蛇形机器人基部位姿进行适当的调整。若求解成功，则基于可行解规划关节运动。

为了验证算法的有效性，进行了蛇形机器人末端捕捉小球的仿真，如图3所示。仿真过程中蛇形机器人完成从初始状态到捕捉到小球1，然后再捕捉小球2。蛇形机器人从当前的状态到初始的状态如图3中的(a)到(b)。然后根据已知的基部末端的位置，使用分段运动规划策略求解出颈部和头部的关节角。通过笛卡尔空间运动规划方法规划蛇形机器人从图3中的(c)所示状态到(e)所示状态。捕捉球1的过程如图3中的(c)(d)(e)(f)(g)。捕捉小球1末端位置误差为1.96mm。之后先脱离球1，然后用分段运动规划策略将捕捉球2的关节角计算出来。从捕捉到球1的状态转换到捕捉球2的过程如图3中的(h)(i)(j)(k)(l)所示。捕捉小球2末端位置误差为1.8mm。

本发明方法采用7自由度连杆作为超冗余机器人头部可以获得超冗余机器人精确的末端位姿解。

本发明的上述算例仅为详细地说明本发明的计算模型和计算流程，而并非是对本发明的实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动，这里无法对所有的实施方式予以穷举，凡是属于本发明的技术方案所引伸出的显而易见的变化或变动仍处于本发明的保护范围之列。

Claims (10)

1.基于分段运动规划策略的机器人末端精确控制方法，其特征在于，针对超冗余机器人进行末端精确控制，所述超冗余机器人为蛇形机器人，包括头部、颈部和基部，头部和基部通过颈部连接；

针对超冗余机器人进行末端精确控制的过程包括以下步骤：

S1、通过平面圆弧背脊曲线确定超冗余机器人的基部的关节角；

S2、确定灵活工作空间的中心：

采用TRAC-IK算法计算超冗余机器人头部工作空间，计算出的头部工作空间共有N_p个点，以其中的第i个空间点p_i为球心，半径R_p构建球面S_i，i＝1,2,3,…,N_p；在S_i上均匀分布N_s个点其中第k个点为p_i,k，k＝1,2,…,N_s；以

为Z轴方向，以p_i,k为原点构建坐标系W_i,k，计算过程如下：

其中，

分别为W_i，k坐标系Z轴、X轴、Y轴单位向量，W_i，k坐标系原点为p_i,k；v_set为用户自定义三维单位向量；则每个p_i构成的球面上均匀分布Ns个期望位姿

使用TRAC-IK算法对每个求取逆运动学解，假设Ns个姿态能求取m个解；基于工作空间，根据可达度指标D确定灵活工作空间，进而确定出灵活工作空间的中心；

S3、将超冗余机器人头部的灵活工作空间中心与头部末端期望位置重合，确定头部基座原点即颈部末端的期望位置，使用最优化方法使颈部末端实际的位置和末端局部坐标系X轴方向靠近期望的位置和方向，进而解出颈部的关节角：

首先确定颈部末端连杆的期望位置：

头部的基坐标系也是颈部的末端坐标系；设灵活工作空间的中心点、头部的基坐标系原点和基部的末端点分别为G₀、B₀和P₀，则有

L_c＝‖B₀-G₀‖ (5)

P_d＝G₀-L_cr_d (7)

其中，L_c为灵活工作空间的中心点到头部的基坐标系原点的距离；P_d为颈部末端连杆的期望位置；r_d为颈部末端连杆的期望方向；

然后，颈部关节角的求解问题便转化为颈部末端姿态与期望位姿误差最小的最优化问题；假设颈部有n_c个关节，颈部关节的关节角为θ_k,θ_k+1,....,θ_k+nc-1，因此有

其中，^kT_k+1为第k+1关节坐标系到第k个坐标系的齐次变换矩阵；E_n为目标函数；P_a和r_a分别为颈部末端连杆的实际位置和方向；h是权重系数；

和

分别为关节角的关节限位下限和上限；

S4、将颈部的末端作为头部的基座采用TRAC-IK逆运动学算法求解出头部的关节角。

2.根据权利要求1所述的基于分段运动规划策略的机器人末端精确控制方法，其特征在于，通过平面圆弧背脊曲线确定超冗余机器人的基部的关节角的过程包括以下步骤：

假设基部的背脊曲线的长度为l，通过离散化半径为r的圆弧获得基部的关节角；对于基部的背脊曲线，有

其中，θ为关节角度，r为基部圆弧背脊曲线的半径；S_c为基部的背脊曲线围成的接触多边形面积；当θ＝π，S_c有最大值为1/2πr²；

当基部的背脊曲线确定后，通过离散化背脊曲线获得基部关节角。

3.根据权利要求2所述的基于分段运动规划策略的机器人末端精确控制方法，其特征在于，通过离散化背脊曲线获得基部关节角的过程包括以下步骤：

采取基于曲率挠率积分的离散化方法计算基部关节角，基部关节角包括侧摆关节和俯仰关节关节角：

式中，Δs为侧摆关节连接的两个连杆的长度之和；κ(s)、κ_c(s)、κ_w(s)分别为基部平面背脊曲线曲率在弧长s处的曲率、曲率在俯仰平面和侧摆平面的分量；θ_yaw、θ_pitch分别为侧摆关节和俯仰关节的关节角；

带入

得

4.根据权利要求1、2或3所述的基于分段运动规划策略的机器人末端精确控制方法，其特征在于，所述可达度指标如下：

其中，m为单个工作空间点构成球面分布的Ns期望位姿能求出解的数目。

5.根据权利要求4所述的基于分段运动规划策略的机器人末端精确控制方法，其特征在于，根据可达度指标D确定灵活工作空间的过程中，选取可达度指标D等于100的工作空间作为灵活工作空间。

6.根据权利要求5所述的基于分段运动规划策略的机器人末端精确控制方法，其特征在于，确定灵活工作空间的中心的过程包括以下步骤：

采用k-means++的方法进行灵活工作空间中心的筛选，进而确定出灵活工作空间的中心。

7.根据权利要求6所述的基于分段运动规划策略的机器人末端精确控制方法，其特征在于，以p_i,k为原点构建坐标系W_i,k的过程中，若计算出的

则将v_set重新设置为[0,1,0]。

8.根据权利要求7所述的基于分段运动规划策略的机器人末端精确控制方法，其特征在于，用户自定义三维单位向量v_set设置为[1,0,0]。

9.一种计算机存储介质，其特征在于，所述存储介质中存储有至少一条指令，所述至少一条指令由处理器加载并执行以实现如权利要求1至8任意一项所述的基于分段运动规划策略的机器人末端精确控制方法。

10.一种基于分段运动规划策略的机器人末端精确控制设备，其特征在于，所述设备包括处理器和存储器，所述存储器中存储有至少一条指令，所述至少一条指令由处理器加载并执行以实现如权利要求1至8任意一项所述的基于分段运动规划策略的机器人末端精确控制方法。

Images