CN111975777A - 一种基于Radau伪谱的机器人关节空间自适应轨迹规划方法 - Google Patents

Abstract

一种基于Radau伪谱的机器人关节空间自适应轨迹规划方法，属于机器人控制技术领域。本发明为了解决轨迹规划中离散计算精度与计算量之间的矛盾问题。本发明基于机器人动力学模型的基本形式，根据边界任务要求设置性能函，然后采用LGR节点作为连续问题离散化的配点方式，LGR配点散落在半开区间[‑1,1)或者(‑1,1]内，包含了一个边界点，通过对时间区间重新划分并调整子区间内节点个数，直到求解精度满足要求或达到最大迭代次数。这种方法效率更高，可以节约大量的计算量，节省计算时间，提高系统实时性。本发明主要用于机器人关节空间自适应轨迹规划。

Description

一种基于Radau伪谱的机器人关节空间自适应轨迹规划方法

技术领域

本发明涉及一种机器人关节空间自适应轨迹规划方法。属于机器人控制技术领域。

背景技术

目前机器人关节空间的轨迹规划多是基于机器人运动学的轨迹规划，这种规划方法只是简单的规划出机器人关节运动的轨迹，通过对关节角速度、角加速度进行设计保证机器人在运动过程中的平稳。但是这种规划主要从运动学出发，没有考虑动力学的影响，对于多关节同时规划或者有燃料和时间约束的规划问题无法求解。

在动力学基础上的轨迹规划方法主要分为两类，一类是通过对哈密顿函数求导得到最优控制必要条件，然后通过数值计算方法进行最优解的计算。这种方法的优点在于通过性能函数直接寻优，计算精度高、计算结果满足最优控制的一阶必要条件。但这种方法收敛域较小、收敛速度慢，并且受初始状态的影响较大。此外，这种计算方法需要计算微分，对于状态方程比较复杂、边界约束强的问题无法求解。另外一类直接将连续最优控制问题转换成离散轨迹规划问题，这种方法受猜测初值的影响较小、收敛半径大、收敛速度快，并且不需要协态变量的信息。但这种方法精度依赖于离散点的数量以及配点方式。

伪谱法是基于正交多项式根为配点的轨迹规划问题离散解法，其利用拉格朗日插值的方式将续优化问题离散化，从而进行求解。目前应用较广的主要是Gauss伪谱法，其将整个时间区间转化成[-1,1]，并选择分布于(-1,1)内的Legendre多项式的零点作为离散节点。但这种方法在求解轨迹规划问题的时候，为了保证精度的要求往往需要增加很多节点。这些节点的增加会导致插值多项式阶次的提高，从而增大计算量，这种时间代价在一些实时性要求比较高的场合是一个非常致命的缺点。此外采用Gauss节点作为插值多项式的节点进行离散时，没有考虑到边界点的作用，这在解决单一时间段内的轨迹规划时没有问题。但是当运动过程不单一，整个运动过程包含很多子段时，无法处理段与段之间的连接问题。

发明内容

本发明是为了解决轨迹规划中离散计算精度与计算量之间的矛盾问题。

一种基于Radau伪谱的机器人关节空间自适应轨迹规划方法，包括以下步骤：

步骤一、基于机器人动力学模型的基本形式，选取关节角度和关节角速度为状态变量将机器人动力学模型描述为状态方程形式

其中

q_m表示关节角度，

表示q_m的一阶导数，u为关节力矩，t为时间；

步骤二、根据边界任务要求设置性能函数J，并将状态和力矩约束统一成路径约束C(x(t),u(t),t)≤0；

步骤三、对[t₀,t_f]划分为S段得到[t₀,t₁,...t_S-1,t_f]，对每段子区间[t_s-1,t_s]采用公式

进行时间标准化处理，变换到[-1,1]，其中s＝1，2，3，...，S；将最优控制问题标准化为：

C(x(τ),u(τ),τ)≤0，E(x(-1),t₀,x(1),t_f)＝0，

E(x(-1),t₀,x(1),t_f)＝0为标准化后的边界约束；

步骤四、选择s段内的初始配点个数N_s即第s段内节点为

取中间点

作为采样时间点；

步骤五、针对每一子区间[t_s-1,t_s]，在LGR节点处离散化处理：

其中，(τ₁,...,τ_N-1,τ_f)为LGR节点，D_k,i为离线RPM微分矩阵中的元素，ω_i为积分权重，L_i(τ)为拉格朗日插值函数，

为不包含初始时刻的插值多项式；

步骤六、求取当前迭代下s段内计算误差：

其中，

为离线RPM微分矩阵，

为N_s×n的状态矩阵，

为状态量在采样点处的值；

为N_s×m的控制矩阵，

为控制量在采样点处的值；R^(s)为N_s×n的误差矩阵，其内每个元素表示状态量在取样点处的绝对误差；

步骤七、计算机器人相对误差判别矩阵

其中，

为R^(s)中第i列中最大误差，

为误差均值；

步骤八、比较δ_max＝max(δ^(s))和期望精度ε_d，δ_max≤ε_d表示当前计算误差满足要求，迭代结束；δ_max＞ε_d则判断是否到达最大迭代次数，若未到最大迭代次数，则增加当前子区间内的LGR节点个数或者对当前子区间重新分段，进入步骤九；

步骤九、计算当前迭代曲率Δ^(s)，如果Δ^(s)＜Δ_max，Δ_max为门槛系数，误差曲率满足要求，且需要增加节点数

小于最大可增加节点数，则转步骤四对子区间内节点重新配置；若Δ^(s)≥Δ_max或者

则需要转步骤三进一步划分子区间。

进一步地，步骤一所述的基于机器人动力学模型的基本形式如下：

其中，

为惯量矩阵，

为离心率和哥氏力矩阵，

是空间重力矢量。

进一步地，步骤二所述的根据边界任务要求设置性能函数吐下：

边界约束：E(x(t₀),t₀,x(t_f),t_f)＝0

控制力矩约束：u∈[u_min,u_max]

状态约束：x∈[x_min,x_max]

其中，t₀,t_f表示初始时刻和终端时刻，χ(x(t₀),t₀,x(t_f),t_f)为始末时间指标函数，即对时间的要求；ζ(x(t),u(t),t)为状态或力矩指标项，u_min,u_max表示了控制力矩的界值，x_min,x_max表示状态量的上下限，即关节角度和关节角速度的上下限；σ₁、σ₂为权重系数。

进一步地，步骤三所述将最优控制问题标准化后的性能函数J如下：

进一步地，步骤五所述的拉格朗日插值函数

进一步地，步骤九所述的计算当前迭代曲率如下：

其中，x^(s)(·)为状态量在采样点处的值，τ_i为LGR节点。

进一步地，步骤九所述的需要增加的节点数

其中<·>表示向上取整。

进一步地，步骤九所述的若Δ^(s)≥Δ_max或者

则需要转步骤三进一步划分子区间的过程中，区间个数为n_k＝2<log₁₀(Δ_s)-log₁₀(ε_d)>，第i个新分段点位置满足

其中c为常数。

有益效果：

本发明采用LGR节点作为连续问题离散化的配点方式，不同于Gauss伪谱法，LGR配点散落在半开区间[-1,1)或者(-1,1]内，包含了一个边界点。这种选点类型可以处理区间分段后，各子区间在边界处的连接问题。本发明所提自适应主要包括对时间区间重新划分和调整子区间内节点个数，直到求解精度满足要求或达到最大迭代次数。这种方法效率更高，可以节约大量的计算量，节省计算时间，提高系统实时性。

传统的伪谱法计算轨迹规划问题时，往往采用固定的节点数目和节点分布方式，因此节点个数往往选的较多，以保证计算的精度。但当任务对精度的要求降低时，由于仍采用原来的配点个数，造成不必要的计算量，对系统的实时性不利。本发明根据任务要求精度进行迭代计算，在满足精度要求的情况时即停止计算，避免了计算量损失，解决了轨迹规划中离散计算精度与计算量之间的矛盾问题，可以适应多种任务情景。

附图说明

图1为具体实施方式一的流程图；

图2为实施例中初始节点为20、迭代1次的计算误差；

图3为实施例中初始节点为20、迭代2次的计算误差；

图4为实施例中初始节点为20、迭代3次的计算误差。

具体实施方式

具体实施方式一：参照图1具体说明本实施方式，

本实施方式所述的一种基于Radau伪谱的机器人关节空间自适应轨迹规划方法，包括：

步骤一、基于机器人动力学模型的基本形式，选取关节角度和关节角速度为状态变量将机器人动力学模型描述为状态方程形式

其中

机器人动力学模型的基本形式如下：

其中，

为惯量矩阵，

为离心率和哥氏力矩阵，

是空间重力矢量，q_m表示关节角度，

表示q_m的一阶导数，u为关节力矩；

步骤二、根据边界任务要求设置性能函数：

边界约束：E(x(t₀),t₀,x(t_f),t_f)＝0

控制力矩约束：u∈[u_min,u_max]

状态约束：x∈[x_min,x_max]

其中，t₀,t_f表示初始时刻和终端时刻，χ(x(t₀),t₀,x(t_f),t_f)为始末时间指标函数，即对时间的要求；ζ(x(t),u(t),t)为状态或力矩指标项，u_min,u_max表示了控制力矩的界值，x_min,x_max表示状态量的上下限，即关节角度和关节角速度的上下限；σ₁、σ₂为权重系数；

将状态和力矩约束统一成路径约束：C(x(t),u(t),t)≤0；

步骤三、对[t₀,t_f]划分为S段得到[t₀,t₁,...t_S-1,t_f]，对每段子区间[t_s-1,t_s]采用公式

进行时间标准化处理，变换到[-1,1]，其中s＝1，2，3，...，S；将最优控制问题标准化为：

C(x(τ),u(τ),τ)≤0，E(x(-1),t₀,x(1),t_f)＝0，

第一次迭代计算是按照给定的节点来的，至少是给定节点的个数，如果计算的不满足要求便会在后续的循环迭代过程中重新划分区间以及重新对节点进行分配。另外这个可以解决分段问题的优化，就是给定的动力学是不连续的，在一段时间是某个方程，另外一段时间是令一个方程，Gauss伪谱法对整个区间划分，出现分段函数的情况，就只能一个区间一个区间的计算，保证不了整个过程情况下的最优；用本发明的方法，相当于再取一个中间时刻，可以解决多段最优问题。

步骤四、选择s段内的初始配点个数N_s即第s段内节点为

取中间点

作为采样时间点；

[t_s-1,t_s]是一个子区间，在这个子区间上是进行了标准化的。在迭代的过程中，t_s这个初始的分段点需要人为设定，后面迭代不满足要求时，再根据自适应率进行计算。

的上角标表示的是参数为s子区间内的配点，配点数是每个子区间内插值多项式的次数，节点数越高，插值多项式的阶次越高，计算量相对就大，如果要想计算误差小，往往是需要次数较高的插值多项式的，这也是gauss伪谱法的问题。对于Gauss伪谱法，就是在整个时间区间进行插值，这样要保证精度，往往要的节点个数就非常高，这样计算起来比较困难。本发明的方法中进行了分段，多项式的个数增加了，但是每个多项式的阶次都不是很高，从而能够极大地减少计算量，提高效率。

此处还进行了选择采样时间点的操作，在配点对应的节点计算的时候都是根据动力学计算的，是真实值，然后根据这些真实的点进行插值，得到一个多项式(曲线)，这样在非节点处的值就会和用动力学计算出来的值有误差，本发明中取两个节点的中间作为采样时间点，通过算这些点的误差反映采用离散方法的精度(在后面的仿真图里也能够看到)。

步骤五、针对每一子区间[t_s-1,t_s]，在LGR节点处离散化处理：

其中，(τ₁,...,τ_N-1,τ_f)为LGR节点，D_k,i为离线RPM微分矩阵中的元素，ω_i为积分权重，

为拉格朗日插值函数，

为不包含初始时刻的插值多项式；

(τ₁,...,τ_N-1,τ_f)这些点是标准化的点。

步骤六、求取当前迭代下s段内计算误差：

其中，

为离线RPM微分矩阵，

为N_s×n的状态矩阵，

为状态量在采样点处的值；

为N_s×m的控制矩阵，

为控制量在采样点处的值；R^(s)为N_s×n的误差矩阵，其内每个元素表示状态量在取样点处的绝对误差；

这里的R^(s)就是离散的状态方程(插值得到)和真实的动力学方程之间的误差，然后选择最大的误差。

步骤七、计算机器人相对误差判别矩阵

其中，

为R^(s)中第i列中最大误差，

为误差均值；

步骤八、比较δ_max＝max(δ^(s))和期望精度ε_d，δ_max≤ε_d表示当前计算误差满足要求，迭代结束；δ_max＞ε_d则判断是否到达最大迭代次数，若未到最大迭代次数，则增加当前子区间内的LGR节点个数或者对当前子区间重新分段，进入步骤九；

步骤九、计算当前迭代曲率：

给定门槛系数Δ_max，如果Δ^(s)＜Δ_max，误差曲率满足要求，且需要增加节点数

小于最大可增加节点数，则转步骤四对子区间内节点重新配置，其中<·>表示向上取整；若Δ^(s)≥Δ_max或者

则需要转步骤三进一步划分子区间，区间个数n_k＝2<log₁₀(Δ_s)-log₁₀(ε_d)>，第i个新分段点位置满足

其中c为常数。

在增加节点时有限制，就是总的节点少于最大节点要求。这里迭代曲率选择了相对值，就是曲率的最大值除上了平均曲率，这样算整个区间一个相对的值，避免了误算。另外这里用误差大小判断增加节点个数，用当前曲率和最大曲率的插值判断重新分段点的个数，并给出了分段点的位置，这个与现有技术是不同的。

机器人在执行任务过程中，针对不同任务，往往有不同的精度要求。但传统的伪谱法未将时间区间进行分段，往往采用固定的节点数量进行对连续问题进行离散，采用这种固定节点的方式经常会出现精度不满足要求，或者插值多项式阶次较高导致计算量大的问题。本发明采用Legendre-Gauss-Radau(LGR)节点作为伪谱法插值计算时的离散点，并根据求解精度和多项式次数，利用自适应率对时间区间和离散点数量进行重新分配，以解决机器人关节空间轨迹规划问题。

本发明采用LGR节点作为连续问题离散化的配点方式，不同于Gauss伪谱法，LGR配点散落在半开区间[-1,1)或者(-1,1]内，包含了一个边界点。这种选点类型可以处理区间分段后，各子区间在边界处的连接问题。本发明所提自适应主要包括对时间区间重新划分和调整子区间内节点个数，直到求解精度满足要求或达到最大迭代次数。这种方法效率更高，可以节约大量的计算量，节省计算时间，提高系统实时性。

实施例

针对平面两连杆漂浮机械臂，通过本发明所提方法求解轨迹规划问题。使用Legendre-Gauss-Radau(LGR)节点作为连续问题离散化配点，即通过非固定节点的方式，将配点分配到半开区间[-1,1)或者(-1,1]内。在解决子区间边界处连接的问题后，对各个子区间自适应划分并自适应调整节点数量，来满足精度和指定时间内收敛的要求。本发明相对于多项式插值法等传统方法降低了计算量和计算时间，并在一定程度上降低了较高的插值多项式阶次提高了系统实时性，主要步骤如下：

步骤一：忽略重力因素，两连杆漂浮机器人动力学方程

转换为状态空间形式

矩阵行使及说明见发明内容步骤一。

步骤二：零初始时刻下,考虑运动时间燃料最优，关节转角在±60°之间，关节角速度在±90°/s以内，控制量在±10N·m内,路径满足

边界条件选择x_f＝45°,

性能函数选取为

其中σ₁,σ₂为表示各性能项权值.

步骤三：考虑到边界约束、控制约束和状态约束均为常值,其变换后形式不变.则上述最优控制问题标准化为：

步骤四：对[t₀,t_f]划分为s段得到[t₀,t₁,...t_s-1,t_f]，对每段[t_s-1,t_s]采用公式

进行时间标准化处理。

步骤五：选择s段内的初始配点个数N_s即第s段内节点为

取中间点

作为采样时间点。

步骤六：针对每一子区间，在LGR节点处离散化处理：

其中(τ₁,...,τ_N-1,τ_f)为LGR节点，D_k,i为离线RPM微分矩阵，ω_i为积分权重计算公式，

为拉格朗日插值函数，

为不包含初始时刻的插值多项式。

步骤七：求取当前迭代下s段内计算误差

其中

为N_s×4的状态矩阵，

为N_s×2的控制矩阵。R^(s)为N_s×4的误差矩阵，其内每个元素表示状态量在取样点处的绝对误差。

步骤八：计算系统相对误差判别函数

为R^(s)中第i列中最大误差，

为误差均值。

步骤九：比较δ_max＝max(δ^(s))和期望精度ε_d。δ_max＜ε_d表示当前计算误差满足要求，迭代结束；δ_max＞ε_d则判断是否到达最大迭代次数，若未到最大迭代次数，增加节点或者重新分段；进入步骤十。

步骤十：计算当前迭代曲率：

给定门槛系数Δ_max，如果Δ^(s)＜Δ_max，误差曲率满足要求，且需要增加节点数

小于最大可增加节点数，则转步骤五对子区间内节点重新配置，其中<·>表示向上取整。若Δ^(s)≥Δ_max或者

则需要转步骤四进一步划分子区间，区间个数n_k＝2<log₁₀(Δ_s)-log₁₀(Δ_max)>，第i个新分段点位置满足

本发明所提关节空间轨迹离散规划方法应用在两连杆漂浮机器人中，计算误差曲线如图2-4所示。实际应用中对节点个数和迭代次数进行了调节，且本发明重点在于轨迹规划的离散计算方法，因此给出了节点20、迭代次数1-3时的计算误差曲线。

图2-图4为最大迭代次数不同的情况下系统的误差曲线，注意到随着迭代次数的增加，整个过程中总的节点个数大量增加，但是每一段区间内节点的个数并未有明显增大，表示每段区间内的多项式次数仍在较低水平，其计算量增加有限。但随着迭代次数增大，总的计算误差迅速降低，计算精度有明显提升。

本发明还可有其它多种实施例，在不背离本发明精神及其实质的情况下，本领域技术人员当可根据本发明作出各种相应的改变和变形，但这些相应的改变和变形都应属于本发明所附的权利要求的保护范围。

Claims (8)

1.一种基于Radau伪谱的机器人关节空间自适应轨迹规划方法，其特征在于，包括：

步骤一、基于机器人动力学模型的基本形式，选取关节角度和关节角速度为状态变量将机器人动力学模型描述为状态方程形式

其中

q_m表示关节角度，

表示q_m的一阶导数，u为关节力矩，t为时间；

步骤二、根据边界任务要求设置性能函数J，并将状态和力矩约束统一成路径约束C(x(t),u(t),t)≤0；

步骤三、对[t₀,t_f]划分为S段得到[t₀,t₁,...t_S-1,t_f]，对每段子区间[t_s-1,t_s]采用公式

进行时间标准化处理，变换到[-1,1]，其中s＝1，2，3，...，S；将最优控制问题标准化为：

C(x(τ),u(τ),τ)≤0，E(x(-1),t₀,x(1),t_f)＝0，

E(x(-1),t₀,x(1),t_f)＝0为标准化后的边界约束；

步骤四、选择s段内的初始配点个数N_s即第s段内节点为

取中间点

作为采样时间点；

步骤五、针对每一子区间[t_s-1,t_s]，在LGR节点处离散化处理：

其中，(τ₁,...,τ_N-1,τ_f)为LGR节点，D_k,i为离线RPM微分矩阵中的元素，ω_i为积分权重，L_i(τ)为拉格朗日插值函数，

为不包含初始时刻的插值多项式；

步骤六、求取当前迭代下s段内计算误差：

其中，

为离线RPM微分矩阵，

为N_s×n的状态矩阵，

为状态量在采样点处的值；

为N_s×m的控制矩阵，

为控制量在采样点处的值；R^(s)为N_s×n的误差矩阵，其内每个元素表示状态量在取样点处的绝对误差；

步骤七、计算机器人相对误差判别矩阵

其中，

为R^(s)中第i列中最大误差，

为误差均值；

步骤八、比较δ_max＝max(δ^(s))和期望精度ε_d，δ_max≤ε_d表示当前计算误差满足要求，迭代结束；δ_max＞ε_d则判断是否到达最大迭代次数，若未到最大迭代次数，则增加当前子区间内的LGR节点个数或者对当前子区间重新分段，进入步骤九；

步骤九、计算当前迭代曲率Δ^(s)，如果Δ^(s)＜Δ_max，Δ_max为门槛系数，误差曲率满足要求，且需要增加节点数

小于最大可增加节点数，则转步骤四对子区间内节点重新配置；若Δ^(s)≥Δ_max或者

则需要转步骤三进一步划分子区间。

2.根据权利要求1所述的一种基于Radau伪谱的机器人关节空间自适应轨迹规划方法，其特征在于，步骤一所述的基于机器人动力学模型的基本形式如下：

其中，

为惯量矩阵，

为离心率和哥氏力矩阵，

是空间重力矢量。

3.根据权利要求2所述的一种基于Radau伪谱的机器人关节空间自适应轨迹规划方法，其特征在于，步骤二所述的根据边界任务要求设置性能函数吐下：

边界约束：E(x(t₀),t₀,x(t_f),t_f)＝0

控制力矩约束：u∈[u_min,u_max]

状态约束：x∈[x_min,x_max]

其中，t₀,t_f表示初始时刻和终端时刻，χ(x(t₀),t₀,x(t_f),t_f)为始末时间指标函数，即对时间的要求；ζ(x(t),u(t),t)为状态或力矩指标项，u_min,u_max表示了控制力矩的界值，x_min,x_max表示状态量的上下限，即关节角度和关节角速度的上下限；σ₁、σ₂为权重系数。

4.根据权利要求3所述的一种基于Radau伪谱的机器人关节空间自适应轨迹规划方法，其特征在于，步骤三所述将最优控制问题标准化后的性能函数J如下：

5.根据权利要求4所述的一种基于Radau伪谱的机器人关节空间自适应轨迹规划方法，其特征在于，步骤五所述的拉格朗日插值函数

6.根据权利要求1至5之一所述的一种基于Radau伪谱的机器人关节空间自适应轨迹规划方法，其特征在于，步骤九所述的计算当前迭代曲率如下：

其中，x^(s)(·)为状态量在采样点处的值，τ_i为LGR节点。

7.根据权利要求6所述的一种基于Radau伪谱的机器人关节空间自适应轨迹规划方法，其特征在于，步骤九所述的需要增加的节点数

其中<·>表示向上取整。

8.根据权利要求7所述的一种基于Radau伪谱的机器人关节空间自适应轨迹规划方法，其特征在于，步骤九所述的若Δ^(s)≥Δ_max或者

则需要转步骤三进一步划分子区间的过程中，区间个数为n_k＝2<log₁₀(Δ_s)-log₁₀(ε_d)>，第i个新分段点位置满足

其中c为常数。

Images