CN114347031A - 一种考虑时变时滞的移动机械臂避碰控制方法及装置 - Google Patents

Abstract

本申请实施例属于人工智能中的移动移动机械臂控制技术领域，涉及一种考虑时变时滞的移动机械臂避碰控制方法及装置。本申请通过建立移动机械臂的时滞动力学模型，并通过估测算法获取系统的时变时滞，消除不确定网络延时对系统控制性能的影响；通过构建考虑时滞的模型预测控制算法，对时滞动力学模型进行控制输入的优化，考虑时滞的模型预测控制提高控制精度；同时，在进行避碰轨迹生成的时候，将系统的时变时滞信息加入到目标函数中，从而可以保证系统的安全性。

Description

一种考虑时变时滞的移动机械臂避碰控制方法及装置

技术领域

本申请涉及人工智能中的移动机械臂控制技术领域，尤其涉及一种考虑时变时滞的移动机械臂避碰控制方法及装置。

背景技术

移动机械臂由于其显著的可操作性和灵活性，可以从事搬运、检验、加工、装配、服务等方面的工作。在危险、枯燥、恶劣的工作环境替代人力、减轻劳动强度、提高效率等方面具有极大的优越性。移动移动机械臂的研究涉及许多内容，如环境感知、决策规划、作业调度、视觉及控制等。其中控制是移动移动机械臂运动的关键技术之一，对移动移动机械臂的控制研究具有重要意义。

目前，针对移动机械臂的控制研究主要集中于理论分析，较少应用于实际系统。同时，虽然已经有大量关于移动移动机械臂的跟踪控制研究，许多学者提出基于运动学或动力学的跟踪控制器，但大多是基于两轮驱动或汽车式集中驱动式移动移动机械臂，针对考虑上装机械臂冗余特性下执行器的避碰控制研究较少。

然而考虑到动态运行环境的复杂性，目前移动机械臂的运动控制仍然存在以下问题：虽然传统方法有针对移动移动机械臂的局部路径重规划方法，但是无法直接应用于集成上装机械臂的移动机械臂的避碰操作，系统安全性难以得到进一步保障，由此可见，传统的移动机械臂运动控制方法存在系统安全性较低的问题。

发明内容

本申请实施例的目的在于提出一种考虑时变时滞的移动机械臂避碰控制方法及装置，以解决传统的移动机械臂运动控制方法存在系统安全性较低的问题。

为了解决上述技术问题，本申请实施例提供一种考虑时变时滞的移动机械臂避碰控制方法，采用了如下所述的技术方案：

接收传感器发送的与移动机械臂相对应的原始控制指令；

获取与所述移动机械臂相对应的基于时变时滞的状态模型；

根据估测算法对所述状态模型进行时滞纠偏操作，得到时变时滞信息；

获取与所述时变时滞信息相对应的模型预测控制器；

根据所述模型预测控制器对所述原始控制指令进行求解操作，得到控制输入信号；

根据所述时变时滞信息以及所述控制输入信号计算与所述移动机械臂相对应的避碰轨迹；

根据所述避碰轨迹对所述移动机械臂进行控制操作。

本申请实施例另一方面提供一种考虑时变时滞的移动机械臂避碰控制装置，包括下述模块：

指令接收模块，用于接收传感器发送的与移动机械臂相对应的原始控制指令；

模型获取模块，用于获取与移动机械臂相对应的基于时变时滞的状态模型；

时滞纠偏模块，用于根据估测算法对状态模型进行时滞纠偏操作，得到时变时滞信息；

控制器获取模块，用于获取与时变时滞信息相对应的模型预测控制器；

求解模块，用于根据模型预测控制器对原始控制指令进行求解操作，得到控制输入信号；

避碰轨迹计算模块，用于根据时变时滞信息以及控制输入信号计算与移动机械臂相对应的避碰轨迹；

操控模块，用于根据避碰轨迹对移动机械臂进行控制操作。

与现有技术相比，本申请实施例主要有以下有益效果：

本申请提供了一种考虑时变时滞的移动机械臂避碰控制方法及装置，方法包括：接收传感器发送的与移动机械臂相对应的原始控制指令；获取与所述移动机械臂相对应的基于时变时滞的状态模型；根据估测算法对所述状态模型进行时滞纠偏操作，得到时变时滞信息；获取与所述时变时滞信息相对应的模型预测控制器；根据所述模型预测控制器对所述原始控制指令进行求解操作，得到控制输入信号；根据所述时变时滞信息以及所述控制输入信号计算与所述移动机械臂相对应的避碰轨迹；根据所述避碰轨迹对所述移动机械臂进行控制操作。本申请通过建立移动机械臂的时滞运动学模型，并通过估测算法获取系统的时变时滞，消除不确定网络延时对系统控制性能的影响；通过构建考虑时滞的模型预测控制算法，对时滞运动学模型进行控制输入的优化，考虑时滞的模型预测控制提高控制精度的同时，进一步通过引入额外的控制参数提高了运动学控制的柔性；同时，在进行避碰轨迹生成的时候，将系统的时变时滞信息加入到目标函数中，从而可以保证系统的安全性。

附图说明

为了更清楚地说明本申请中的方案，下面将对本申请实施例描述中所需要使用的附图作一个简单介绍，显而易见地，下面描述中的附图是本申请的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1是本申请实施例一提供的考虑时变时滞的移动机械臂避碰控制方法的实现流程图；

图2是本申请实施例二提供的考虑时变时滞的移动机械臂避碰控制装置的结构示意图。

具体实施方式

除非另有定义，本文所使用的所有的技术和科学术语与属于本申请的技术领域的技术人员通常理解的含义相同；本文中在申请的说明书中所使用的术语只是为了描述具体的实施例的目的，不是旨在于限制本申请；本申请的说明书和权利要求书及上述附图说明中的术语“包括”和“具有”以及它们的任何变形，意图在于覆盖不排他的包含。本申请的说明书和权利要求书或上述附图中的术语“第一”、“第二”等是用于区别不同对象，而不是用于描述特定顺序。

在本文中提及“实施例”意味着，结合实施例描述的特定特征、结构或特性可以包含在本申请的至少一个实施例中。在说明书中的各个位置出现该短语并不一定均是指相同的实施例，也不是与其它实施例互斥的独立的或备选的实施例。本领域技术人员显式地和隐式地理解的是，本文所描述的实施例可以与其它实施例相结合。

为了使本技术领域的人员更好地理解本申请方案，下面将结合附图，对本申请实施例中的技术方案进行清楚、完整地描述。

继续参考图1，示出了本申请实施例一提供的考虑时变时滞的移动机械臂避碰控制方法的实现流程图，为了便于说明，仅示出与本申请相关的部分。

上述的考虑时变时滞的移动机械臂避碰控制方法，包括以下步骤：

步骤S201：接收传感器发送的与移动机械臂相对应的原始控制指令；

步骤S202：获取与移动机械臂相对应的基于时变时滞的状态模型；

步骤S203：根据估测算法对状态模型进行时滞纠偏操作，得到时变时滞信息；

步骤S204：获取与时变时滞信息相对应的模型预测控制器；

步骤S205：根据模型预测控制器对原始控制指令进行求解操作，得到控制输入信号；

步骤S206：根据时变时滞信息以及控制输入信号计算与移动机械臂相对应的避碰轨迹；

步骤S207：根据避碰轨迹对移动机械臂进行控制操作。

在本申请实施例中，考虑移动机械臂底盘系统的横向动力学稳定性，基于时变时滞的状态模型如下：

其中，β表示质心侧偏角；γ表示横摆角速度；m表示所述移动机械臂总质量；v_x表示质心速度v沿x轴的分量；f_ix表示所述移动机械臂前后轮胎的轴向力；δ表示所述移动机械臂前轮转角；f_iy表示所述移动机械臂前后轮胎的侧向力；L_f表示所述移动机械臂质心到前轴的距离；L_r表示所述移动机械臂质心到后轴的距离；M_ω表示所述移动机械臂所有驱动轮纵向力共同对所述移动机械臂质心产生的横摆力矩，I_z表示系系统惯量。

在本申请实施例中，横摆力矩由下式决定：

M_ω＝0.5w_d(f_xfr-f_xfl)cosδ+0.5w_d(f_xrr-f_xrl)

其中，w_d表示底盘车宽，f_xfr,f_xfl,f_xrr,f_xrl分别表示右前轮、左前轮、右后轮、左后轮的驱动力矩

根据小角度假设，cosδ≈1,sinδ≈0，因此可以得到

其中

考虑系统中前向通道和反馈通道存在的总时滞τ(t)，前向反馈通道时滞与反馈通道时滞对系统的影响基本相同。因此，本文将反馈时滞等效至前向通道，即只在控制输入体现时滞，从而得到移动机械臂系统在连续时间框架下时滞状态方程如下：

其中，A和B表示参数矩阵，x(t)表示状态变量，u(t)表示输入参量，τ(t) 表示时变时滞。在理想网络环境下传感器以固定周期采样，向控制器发送测量信号；控制器在接收到传感器信号后立即执行相应控制算法计算出控制命令，再通过CAN/EtherCat等网络将控制命令发送给电机控制器，即电机驱动器；电机驱动器接收到消息后，驱动电机输出力矩并保持当前状态，直到下一周期接收到新的控制命令，因此电机控制器可等效于零阶保持器。在理想网络环境下，传感器采样、网络通信、控制器计算以及底层控制器响应等过程均认为是瞬时，因此，每一个控制命令都是等周期执行。

在本申请实施例中，按照本发明的另一方面，提供了一种对移动机械臂运动学模型的时滞进行无偏估计的方法，表示如下：

在分布式控制系统中，控制节点与执行器节点间通过以太网，现场总线或无线等网络通信。控制节点发送初始信号s(t)，通过网络传输，经过时变网络通信时滞τ_i(t)，另一端执行器节点收到延迟的控制信号s(t-τ_i(t))，然后延迟信号s(t-τ_i(t))直接返回到控制节点，再经过时变网络通信时延τ₀(t)，控制节点收到延迟信号s(t-τ(t))。因此，信号传输过程中总的网络时滞可以表示为：

τ(t)＝τ₀(t)+τ_i(t-τ₀(t))

式中，τ(t)表示总的网络传输时滞，假设其满足τ(t)∈[0,τ_max]，τ_max＞0，τ_max可以大于采样周期。

利用时滞估计器的自定义信号s(t)，与控制信号u(t)一起发送到被控对象的消息接收器，对于延迟信号s(t-τ_i(t))立即传输至平台消息发送器将其返回到时滞估计器。执行器则执行延迟的控制命令u(t-τ_i(t))直到接受到下一次控制命令。通过对自定义的信号s(t)进行时滞估计，从而得到系统时滞，实现时滞估计与原控制系统的解耦。定义估测观测如下：

其中，s表示外部输入的测试信号；由于时滞的存在，接收端接收到的测试信号可表示为

表示估计的时滞；

表示估计的时滞的导数， w(t)为自适应迭代参数。设计如下滑模面：

其中，w(t)定义为：

其中，

表示测试信号的差值；τ(t) 表示实际的时滞；ζ表示预先设定的分数阶阶次，λ(t)和α(t)表示时变系数； w₁(t)表示计算的中间变量；

表示w₁(t)的导数。

优选地，根据超螺旋滑模分析，

其中，Δλ(t)表示参数λ(t)的变化量；所述Δλ(t)利用时滞估计差值的梯度信息进行求解得到，可表示为：

其中，Ω(t)表示调节收敛速度的遗忘因子；

表示计算函数F_τ(λ) 的下降梯度信息；

表示自适应的系数参量。

在本申请实施例中，按照本发明的另一方面，基于构建的运动学时滞模型，提供了一种模型预测控制方法，表示如下：

考虑到系统的时变时滞，定义模型预测滚动优化函数为：

其中，

是

的增量，Q和R为自定义的对角矩阵,N和N_u分别表示预测域和控制域,

表示连续型终端惩罚项，

表示在时域t≤τ≤t+N的系统状态预测值，

表示在时域t≤τ≤t+N控制输入信号的预测变化量。

因此对控制输入信号u_e表示为u_e＝[δM_ω]，可由下式优化计算得到：

其中，

表示终端状态区域；q_e＝[βγ]表示系统的状态值，；Δu_e(τ)表示延时条件下，相邻间两个控制输入信号的差值，即为控制输入信号的变化量，φ表示输入变量的集合；min表示求取最小值，

是模型预测滚动优化函数。

在本申请实施例中，按照本发明的另一方面，对模型预测控制器进行了控制输入的求解，表示如下：

首先，为了保证系统的最优控制性能，使用结合车速和系统曲率，构建多层次模糊规则，对系统的预测域和控制域进行优化求解。多层次模糊切换规则的输出即为最佳的预测域和控制域，多层次模糊切换规则的输出表达式为：

其中，

表示隶属度函数；

和

分别表示

和

的均值；m_g表示模糊规则的数量；n1和n2分别表示两级模糊化的输入变量个数；output(N)表示一级模糊规则的输出信号，即为控制域；output(N_u) 表示二级模糊规则的输出信号，即为预测域；x_i表示模糊规则输入变量，当i＝1 时为移动机械臂运行速度，当i＝2时为运行轨迹参考曲率。因此，结合最优化的控制预测域和控制域，可以通过神经网络优化算法对模型预测模型预测滚动优化函数进行寻优求解，最终保证系统控制输入最优，最终得到控制输入 u_e(t)。

在本申请实施例中，按照本发明的另一方面，基于估测的时变时滞和上装机械臂构型约束，生成系统的避碰轨迹，表示如下：

移动平台实现路径规划的基础建立在一张可提供精确信息的静态环境地图上。移动平台在运行过程中的安全性十分重要，必须保证移动平台不与障碍物发生碰撞。在对移动平台进行路径规划时，为了方便处理，通常将移动平台视为一个点。如果直接忽略移动平台的形状尺寸和最小安全距离等信息，规划的路径势必会与障碍物发生碰撞，故需要对障碍物进行膨胀处理假定移动平台以其几何中心为圆心，其外轮廓的最大回转半径记为d。

在移动平台上建立以回转中心为原点的平面坐标系o-xy，在机械臂上以机械臂基座中心为原点的平面坐标系o'-xy'。设坐标系o'-xy'中有一点 P'(x',y')，点P(x,y)为点P'在坐标系o-xy下的转换点。坐标系o'-xy'与o-xy上点的坐标存在如下转换关系：

式中，Δ为原点o与o'的距离。

根据关于机械臂的正运动学分析，当机械臂处于某一位姿时，机械臂6 个关节在空间坐标系中相对于基座的位置可以确定。考虑到机械臂关节1和关节2一定处于移动平台的回转半径内，故此处不考虑关节1和关节2的空间位置。以基座中心为原点，建立笛卡尔空间坐标系o₀-x₀y₀z₀，可分别计算出关节3、4、5、6的空间坐标值，分别记为 P₃(x₃,y₃,z₃)、P₄(x₄,y₄,z₄)、P₅(x₅,y₅,z₅)、P₆(x₆,y₆,z₆)。在平面坐标系o'-xy'下，忽略各点在z方向的坐标。通过坐标转换，将点P₃、P₄、P₅、P₆变换到坐标系o-xy下。通过平面直角坐标系下的距离公式：

计算P₃、P₄、P₅、P₆到原点o 的距离，分别记为ρ₃、ρ₄、ρ₅、ρ₆。其中，四点中距离原点o最远的距离记为ρ_max，则有

ρ_max＝max(ρ₃、ρ₄、ρ₅、ρ₆)

在本申请实施例中，确定移动移动机械臂的回转半径d_m为：

d_m＝max(d,ρ_max)

式中，d为移动平台的回转半径。

进而，通过考虑系统的时变延时和避碰需求，建立如下的避碰轨迹生成方案：

其中，V表示避障区域，

和

分别表示雷达的避碰距离和常规避碰距离；

表示考虑延时的潜在运行距离；d_ro表示移动机械臂探测到障碍物的实际距离，v_x表示移动机械臂的移动速度；L＞l、

表示权重系数；(x_g,y_g)和(x_o,y_o)分别表示避障后的最优子目标位置和检测到的障碍物位置； l＝max(d,ρ_max)；

避碰路径为：

其中，x_r，y_r，θ_r分别表示x轴方向坐标、y轴方向坐标以及方位角θ的参考值，V表示避障区域的势场函数，

表示求导操作，

最终，通过参考轨迹x_r,y_r,θ_r可推导得到横向动力学参考值。

在本申请的一些可选实施例中，避障区域的势场函数V表示为；

其中，

和

分别表示雷达的避碰距离和常规避碰距离；

表示考虑延时的潜在运行距离；d_ro表示移动机械臂探测到障碍物的实际距离；ζ表示权重系数；(x_g,y_g)和(x_o,y_o)分别表示避障后的最优子目标位置和检测到的障碍物位置；l＝max(d,ρ_max)，其中，d表示移动平台的回转半径；ρ_max表示对机械臂各关节进行膨胀处理后到基座的距离ρ_i的最大值，ρ_max表示为：

ρ_max＝max{ρ_i},i＝1,2,...,N_i

其中，N_i表示移动机械臂的关节数；

x_i和y_i分别表示i关节的x轴方向和y轴方向的坐标值，max表示求取最大值。

在本申请实施例中，提供了一种考虑时变时滞的移动机械臂避碰控制方法：包括：接收传感器发送的与移动机械臂相对应的原始控制指令；获取与移动机械臂相对应的基于时变时滞的状态模型；根据估测算法对状态模型进行时滞纠偏操作，得到时变时滞信息；获取与时变时滞信息相对应的模型预测控制器；根据模型预测控制器对原始控制指令进行求解操作，得到控制输入信号；根据时变时滞信息以及控制输入信号计算与移动机械臂相对应的避碰轨迹；根据避碰轨迹对移动机械臂进行控制操作。本申请通过建立移动机械臂的时滞运动学模型，并通过估测算法获取系统的时变时滞，消除不确定网络延时对系统控制性能的影响；通过构建考虑时滞的模型预测控制算法，对时滞运动学模型进行控制输入的优化，考虑时滞的模型预测控制提高控制精度的同时，进一步通过引入额外的控制参数提高了运动学控制的柔性；同时，在进行避碰轨迹生成的时候，将系统的时变时滞信息加入到目标函数中，从而可以保证系统的安全性。

应该理解的是，虽然附图的流程图中的各个步骤按照箭头的指示依次显示，但是这些步骤并不是必然按照箭头指示的顺序依次执行。除非本文中有明确的说明，这些步骤的执行并没有严格的顺序限制，其可以以其他的顺序执行。而且，附图的流程图中的至少一部分步骤可以包括多个子步骤或者多个阶段，这些子步骤或者阶段并不必然是在同一时刻执行完成，而是可以在不同的时刻执行，其执行顺序也不必然是依次进行，而是可以与其他步骤或者其他步骤的子步骤或者阶段的至少一部分轮流或者交替地执行。

实施例二

进一步参考图2，作为对上述图1所示方法的实现，本申请提供了一种考虑时变时滞的移动机械臂避碰控制装置的一个实施例，该装置实施例与图1 所示的方法实施例相对应，该装置具体可以应用于各种电子设备中。

如图2所示，本实施例的考虑时变时滞的移动机械臂避碰控制装置200 包括：指令接收模块201、模型获取模块202、时滞纠偏模块203、控制器获取模块204、求解模块205、避碰轨迹计算模块206以及操控模块207。其中：

指令接收模块201，用于接收传感器发送的与移动机械臂相对应的原始控制指令；

模型获取模块202，用于获取与移动机械臂相对应的基于时变时滞的状态模型；

时滞纠偏模块203，用于根据估测算法对状态模型进行时滞纠偏操作，得到时变时滞信息；

控制器获取模块204，用于获取与时变时滞信息相对应的模型预测控制器；

求解模块205，用于根据模型预测控制器对原始控制指令进行求解操作，得到控制输入信号；

避碰轨迹计算模块206，用于根据时变时滞信息以及控制输入信号计算与移动机械臂相对应的避碰轨迹；

操控模块207，用于根据避碰轨迹对移动机械臂进行控制操作。

在本申请实施例中，考虑移动机械臂底盘系统的横向动力学稳定性，基于时变时滞的状态模型如下：

其中，β表示质心侧偏角；γ表示横摆角速度；m表示所述移动机械臂总质量；v_x表示质心速度v沿x轴的分量；f_ix表示所述移动机械臂前后轮胎的轴向力；δ表示所述移动机械臂前轮转角；f_iy表示所述移动机械臂前后轮胎的侧向力；L_f表示所述移动机械臂质心到前轴的距离；L_r表示所述移动机械臂质心到后轴的距离；M_ω表示所述移动机械臂所有驱动轮纵向力共同对所述移动机械臂质心产生的横摆力矩，I_z表示系系统惯量。

在本申请实施例中，横摆力矩由下式决定：

M_ω＝0.5w_d(f_xfr-f_xfl)cosδ+0.5w_d(f_xrr-f_xrl)

其中，w_d表示底盘车宽，f_xfr,f_xfl,f_xrr,f_xrl分别表示右前轮、左前轮、右后轮、左后轮的驱动力矩

根据小角度假设，cosδ≈1,sinδ≈0)，因此可以得到

其中

考虑系统中前向通道和反馈通道存在的总时滞τ(t)，前向反馈通道时滞与反馈通道时滞对系统的影响基本相同。因此，本文将反馈时滞等效至前向通道，即只在控制输入体现时滞，从而得到移动机械臂系统在连续时间框架下时滞状态方程如下：

其中，A和B表示参数矩阵，x(t)表示状态变量，u(t)表示输入参量，τ(t) 表示时变时滞。在理想网络环境下传感器以固定周期采样，向控制器发送测量信号；控制器在接收到传感器信号后立即执行相应控制算法计算出控制命令，再通过CAN/EtherCat等网络将控制命令发送给电机控制器，即电机驱动器；电机驱动器接收到消息后，驱动电机输出力矩并保持当前状态，直到下一周期接收到新的控制命令，因此电机控制器可等效于零阶保持器。在理想网络环境下，传感器采样、网络通信、控制器计算以及底层控制器响应等过程均认为是瞬时，因此，每一个控制命令都是等周期执行。

在本申请实施例中，按照本发明的另一方面，提供了一种对移动机械臂运动学模型的时滞进行无偏估计的方法，表示如下：

在分布式控制系统中，控制节点与执行器节点间通过以太网，现场总线或无线等网络通信。控制节点发送初始信号s(t)，通过网络传输，经过时变网络通信时滞τ_i(t)，另一端执行器节点收到延迟的控制信号s(t-τ_i(t))，然后延迟信号s(t-τ_i(t))直接返回到控制节点，再经过时变网络通信时延τ₀(t)，控制节点收到延迟信号s(t-τ(t))。因此，信号传输过程中总的网络时滞可以表示为：

τ(t)＝τ₀(t)+τ_i(t-τ₀(t))

式中，τ(t)表示总的网络传输时滞，假设其满足τ(t)∈[0,τ_max]，τ_max＞0，τ_max可以大于采样周期。

利用时滞估计器的自定义信号s(t)，与控制信号u(t)一起发送到被控对象的消息接收器，对于延迟信号s(t-τ_i(t))立即传输至平台消息发送器将其返回到时滞估计器。执行器则执行延迟的控制命令u(t-τ_i(t))直到接受到下一次控制命令。通过对自定义的信号s(t)进行时滞估计，从而得到系统时滞，实现时滞估计与原控制系统的解耦。定义估测观测如下：

其中，s表示外部输入的测试信号；由于时滞的存在，接收端接收到的测试信号可表示为

表示估计的时滞；

表示估计的时滞的导数， w(t)为自适应迭代参数。设计如下滑模面：

其中，w(t)定义为：

其中，

表示测试信号的差值；τ(t) 表示实际的时滞；ζ表示预先设定的分数阶阶次，λ(t)和α(t)表示时变系数； w₁(t)表示计算的中间变量；

表示w₁(t)的导数。

优选地，根据超螺旋滑模分析，所述时变系数λ(t)和α(t)的定义将根据时滞估测的差值进行求解，具体表示为：

其中，Δλ(t)表示参数λ(t)的变化量；所述Δλ(t)利用时滞估计差值的梯度信息进行求解得到，可表示为：

其中，Ω(t)表示调节收敛速度的遗忘因子；

表示计算函数F_τ(λ) 的下降梯度信息；

表示自适应的系数参量。

在本申请实施例中，按照本发明的另一方面，基于构建的运动学时滞模型，提供了一种模型预测控制方法，表示如下：

考虑到系统的时变时滞，定义模型预测滚动优化函数为：

其中，

是

的增量，Q和R为自定义的对角矩阵,N和N_u分别表示预测域和控制域,

表示连续型终端惩罚项，

表示在时域t≤τ≤t+N的系统状态预测值，

表示在时域t≤τ≤t+N控制输入信号的预测变化量。

因此对控制输入信号u_e表示为u_e＝[δM_ω]，可由下式优化计算得到：

其中，

表示终端状态区域；q_e＝[βγ]表示系统的状态值，；Δu_e(τ)表示延时条件下，相邻间两个控制输入信号的差值，即为控制输入信号的变化量，φ表示输入变量的集合；min表示求取最小值，

是模型预测滚动优化函数。

在本申请实施例中，按照本发明的另一方面，对模型预测控制器进行了控制输入的求解，表示如下：

首先，为了保证系统的最优控制性能，使用结合车速和系统曲率，构建多层次模糊规则，对系统的预测域和控制域进行优化求解。多层次模糊切换规则的输出即为最佳的预测域和控制域，多层次模糊切换规则的输出表达式为：

其中，

表示隶属度函数；

和

分别表示

和

的均值；m_g表示模糊规则的数量；n1和n2分别表示两级模糊化的输入变量个数；output(N)表示一级模糊规则的输出信号，即为控制域；output(N_u) 表示二级模糊规则的输出信号，即为预测域；x_i表示模糊规则输入变量，当 i＝1时为移动机械臂运行速度，当i＝2时为运行轨迹参考曲率。因此，结合最优化的控制预测域和控制域，可以通过神经网络优化算法对模型预测模型预测滚动优化函数进行寻优求解，最终保证系统控制输入最优，最终得到控制输入u_e(t)。

在本申请实施例中，按照本发明的另一方面，基于估测的时变时滞和上装机械臂构型约束，生成系统的避碰轨迹，表示如下：

移动平台实现路径规划的基础建立在一张可提供精确信息的静态环境地图上。移动平台在运行过程中的安全性十分重要，必须保证移动平台不与障碍物发生碰撞。在对移动平台进行路径规划时，为了方便处理，通常将移动平台视为一个点。如果直接忽略移动平台的形状尺寸和最小安全距离等信息，规划的路径势必会与障碍物发生碰撞，故需要对障碍物进行膨胀处理假定移动平台以其几何中心为圆心，其外轮廓的最大回转半径记为d。

在移动平台上建立以回转中心为原点的平面坐标系o-xy，在机械臂上以机械臂基座中心为原点的平面坐标系o'-xy'。设坐标系o'-xy'中有一点 P'(x',y')，点P(x,y)为点P'在坐标系o-xy下的转换点。坐标系o'-xy'与o-xy上点的坐标存在如下转换关系：

式中，Δ为原点o与o'的距离。

根据关于机械臂的正运动学分析，当机械臂处于某一位姿时，机械臂6 个关节在空间坐标系中相对于基座的位置可以确定。考虑到机械臂关节1和关节2一定处于移动平台的回转半径内，故此处不考虑关节1和关节2的空间位置。以基座中心为原点，建立笛卡尔空间坐标系o₀-x₀y₀z₀，可分别计算出关节3、4、5、6的空间坐标值，分别记为 P₃(x₃,y₃,z₃)、P₄(x₄,y₄,z₄)、P₅(x₅,y₅,z₅)、P₆(x₆,y₆,z₆)。在平面坐标系o'-xy'下，忽略各点在z方向的坐标。通过坐标转换，将点P₃、P₄、P₅、P₆变换到坐标系o-xy 下。通过平面直角坐标系下的距离公式：

计算P₃、P₄、P₅、P₆到原点o的距离，分别记为ρ₃、ρ₄、ρ₅、ρ₆。其中，四点中距离原点o最远的距离记为ρ_max，则有

ρ_max＝max(ρ₃、ρ₄、ρ₅、ρ₆)

在本申请实施例中，确定移动移动机械臂的回转半径d_m为：

d_m＝max(d,ρ_max)

式中，d为移动平台的回转半径。

进而，通过考虑系统的时变延时和避碰需求，建立如下的避碰轨迹生成方案：

其中，V表示避障区域，

和

分别表示雷达的避碰距离和常规避碰距离；

表示考虑延时的潜在运行距离；d_ro表示移动机械臂探测到障碍物的实际距离，v_x表示移动机械臂的移动速度；L＞l、

表示权重系数；(x_g,y_g)和(x_o,y_o)分别表示避障后的最优子目标位置和检测到的障碍物位置； l＝max(d,ρ_max)；

避碰路径为：

其中，x_r，y_r，θ_r分别表示x轴方向坐标、y轴方向坐标以及方位角θ的参考值，V表示避障区域的势场函数，

表示求导操作，

最终，通过参考轨迹x_r,y_r,θ_r可推导得到横向动力学参考值。

在本申请的一些可选实施例中，避障区域的势场函数V表示为；

其中，

和

分别表示雷达的避碰距离和常规避碰距离；

表示考虑延时的潜在运行距离；d_ro表示移动机械臂探测到障碍物的实际距离；ζ表示权重系数；(x_g,y_g)和(x_o,y_o)分别表示避障后的最优子目标位置和检测到的障碍物位置；l＝max(d,ρ_max)，其中，d表示移动平台的回转半径；ρ_max表示对机械臂各关节进行膨胀处理后到基座的距离ρ_i的最大值，ρ_max表示为：

ρ_max＝max{ρ_i},i＝1,2,...,N_i

其中，N_i表示移动机械臂的关节数；

x_i和y_i分别表示i关节的x轴方向和y轴方向的坐标值，max表示求取最大值。

在本申请实施例中，提供了一种考虑时变时滞的移动机械臂避碰控制装置200，包括：指令接收模块201，用于接收传感器发送的与移动机械臂相对应的原始控制指令；模型获取模块202，用于获取与移动机械臂相对应的基于时变时滞的状态模型；时滞纠偏模块203，用于根据估测算法对状态模型进行时滞纠偏操作，得到时变时滞信息；控制器获取模块204，用于获取与时变时滞信息相对应的模型预测控制器；求解模块205，用于根据模型预测控制器对原始控制指令进行求解操作，得到控制输入信号；避碰轨迹计算模块206，用于根据时变时滞信息以及控制输入信号计算与移动机械臂相对应的避碰轨迹；操控模块207，用于根据避碰轨迹对移动机械臂进行控制操作。本申请通过建立移动机械臂的时滞运动学模型，并通过估测算法获取系统的时变时滞，消除不确定网络延时对系统控制性能的影响；通过构建考虑时滞的模型预测控制算法，对时滞运动学模型进行控制输入的优化，考虑时滞的模型预测控制提高控制精度的同时，进一步通过引入额外的控制参数提高了运动学控制的柔性；同时，在进行避碰轨迹生成的时候，将系统的时变时滞信息加入到目标函数中，从而可以保证系统的安全性。

通过建立移动机械臂的时滞运动学模型，并通过估测算法获取系统的时变时滞，消除不确定网络延时对系统控制性能的影响；通过构建考虑时滞的模型预测控制算法，对时滞运动学模型进行控制输入的优化，考虑时滞的模型预测控制提高控制精度的同时，进一步通过引入额外的控制参数提高了运动学控制的柔性；同时，在进行避碰轨迹生成的时候，将系统的时变时滞信息加入到目标函数中，从而可以保证系统的安全性。

本申请通过建立移动机械臂的时滞运动学模型，并通过估测算法获取系统的时变时滞，消除不确定网络延时对系统控制性能的影响；通过构建考虑时滞的模型预测控制算法，对时滞运动学模型进行控制输入的优化，考虑时滞的模型预测控制提高控制精度的同时，进一步通过引入额外的控制参数提高了运动学控制的柔性；同时，在进行避碰轨迹生成的时候，将系统的时变时滞信息加入到目标函数中，从而可以保证系统的安全性。

通过以上的实施方式的描述，本领域的技术人员可以清楚地了解到上述实施例方法可借助软件加必需的通用硬件平台的方式来实现，当然也可以通过硬件，但很多情况下前者是更佳的实施方式。

显然，以上所描述的实施例仅仅是本申请一部分实施例，而不是全部的实施例，附图中给出了本申请的较佳实施例，但并不限制本申请的专利范围。本申请可以以许多不同的形式来实现，相反地，提供这些实施例的目的是使对本申请的公开内容的理解更加透彻全面。尽管参照前述实施例对本申请进行了详细的说明，对于本领域的技术人员来而言，其依然可以对前述各具体实施方式所记载的技术方案进行修改，或者对其中部分技术特征进行等效替换。凡是利用本申请说明书及附图内容所做的等效结构，直接或间接运用在其他相关的技术领域，均同理在本申请专利保护范围之内。

Claims (10)

1.一种考虑时变时滞的移动机械臂避碰控制方法，其特征在于，包括下述步骤：

接收传感器发送的与移动机械臂相对应的原始控制指令；

获取与所述移动机械臂相对应的基于时变时滞的状态模型；

根据估测算法对所述状态模型进行时滞纠偏操作，得到时变时滞信息；

获取与所述时变时滞信息相对应的模型预测控制器；

根据所述模型预测控制器对所述原始控制指令进行求解操作，得到控制输入信号；

根据所述时变时滞信息以及所述控制输入信号计算与所述移动机械臂相对应的避碰轨迹；

根据所述避碰轨迹对所述移动机械臂进行控制操作。

2.根据权利要求1所述的考虑时变时滞的移动机械臂避碰控制方法，其特征在于，所述基于时变时滞的状态模型表示为：

其中，β表示质心侧偏角；γ表示横摆角速度；m表示所述移动机械臂总质量；v_x表示底盘质心速度v沿x轴的分量；f_ix表示所述移动机械臂底盘前后轮胎的轴向力；δ表示所述移动机械臂底盘前轮转角；f_iy表示所述移动机械臂底盘前后轮胎的侧向力；L_f表示所述移动机械臂底盘质心到前轴的距离；L_r表示所述移动机械臂底盘质心到后轴的距离；M_ω表示所述移动机械臂所有驱动轮纵向力共同对所述移动机械臂质心产生的横摆力矩，I_z表示系统惯量。

3.根据权利要求2所述的考虑时变时滞的移动机械臂避碰控制方法，其特征在于，所述横摆力矩表示为：

M_ω＝0.5w_d(f_xfr-f_xfl)cosδ+0.5w_d(f_xrr-f_xrl)

其中，w_d表示所述移动机械臂底盘车宽；f_xfr、f_xfl、f_xrr、f_xrl分别表示所述移动机械臂底盘的右前轮、左前轮、右后轮、左后轮的驱动力矩。

4.根据权利要求1所述的考虑时变时滞的移动机械臂避碰控制方法，其特征在于，采用外部输入的测试信号s(t)来进行时滞信息的估计,所述时变时滞信息可由下式计算得到：

其中，s表示外部输入的测试信号；由于时滞的存在，接收端接收到的测试信号可表示为

表示估计的时滞；

表示估计的时滞的导数，w(t)为自适应迭代参数，表示为：

其中，

表示测试信号的差值；τ(t)表示实际的时滞；ζ表示预先设定的分数阶阶次，λ(t)和α(t)表示时变系数；w₁(t)表示计算的中间变量，

表示w₁(t)的导数。

5.根据权利要求4所述的考虑时变时滞的移动机械臂避碰控制方法，其特征在于，所述时变系数λ(t)和α(t)的定义将根据时滞估测的差值进行求解，具体表示为：

其中，Δλ(t)表示参数λ(t)的变化量；所述Δλ(t)利用时滞估计差值的梯度信息进行求解得到，可表示为：

其中，Ω(t)表示调节收敛速度的遗忘因子；

表示计算函数F_τ(λ)的下降梯度信息；

表示自适应的系数参量。

6.根据权利要求1所述的考虑时变时滞的移动机械臂避碰控制方法，其特征在于，所述控制输入信号u_e表示为u_e＝[δ M_ω]，可由下式优化计算得到：

其中，

表示终端状态区域；q_e＝[β γ]表示系统的状态值；Δu_e(τ)表示延时条件下，相邻间两个控制输入信号的差值，即为控制输入信号的变化量，φ表示输入变量的集合；min表示求取最小值，

是模型预测滚动优化函数，定义为：

其中，

是

的增量，Q和R为自定义的对角矩阵,N和N_u分别表示预测域和控制域,

表示连续型终端惩罚项，

表示在时域t≤τ≤t+N的系统状态预测值，

表示在时域t≤τ≤t+N控制输入信号的预测变化量。

7.根据权利要求6所述的考虑时变时滞的移动机械臂避碰控制方法，其特征在于，为了保证系统的最优控制性能，结合移动机械臂运行速度和运行轨迹参考曲率，构建多层次模糊规则，对系统的预测域和控制域进行优化求解。多层次模糊切换规则的输出即为最佳的预测域N和控制域N_u，多层次模糊切换规则的输出表达式为：

其中，

表示隶属度函数；

和

分别表示

和

的均值；m_g表示模糊规则的数量；n1和n2分别表示两级模糊化的输入变量个数；output(N)表示一级模糊规则的输出信号，即为控制域；output(N_u)表示二级模糊规则的输出信号，即为预测域；x_i表示模糊规则输入变量，当i＝1时为移动机械臂运行速度，当i＝2时为运行轨迹参考曲率。因此，结合最优化的控制预测域和控制域，可以通过神经网络优化算法对模型预测模型预测滚动优化函数进行寻优求解，最终保证系统控制输入最优，最终得到控制输入u_e(t)。

8.根据权利要求1所述的考虑时变时滞的移动机械臂避碰控制方法，其特征在于，所述避碰轨迹表示为：

其中，x_r，y_r，θ_r分别表示x轴方向坐标、y轴方向坐标以及方位角θ的参考值，V表示避障区域的势场函数，

表示求导操作，

9.根据权利要求8所述的考虑时变时滞的移动机械臂避碰控制方法，其特征在于，避障区域的势场函数V表示为；

其中，

和

分别表示雷达的避碰距离和常规避碰距离；

表示考虑延时的潜在运行距离；d_ro表示移动机械臂探测到障碍物的实际距离；ζ表示权重系数；(x_g,y_g)和(x_o,y_o)分别表示避障后的最优子目标位置和检测到的障碍物位置；l＝max(d,ρ_max)，其中，d表示移动平台的回转半径；ρ_max表示对机械臂各关节进行膨胀处理后到基座的距离ρ_i的最大值，ρ_max表示为：

ρ_max＝max{ρ_i},i＝1,2,...,N_i

其中，N_i表示移动机械臂的关节数；

x_i和y_i分别表示i关节的x轴方向和y轴方向的坐标值，max表示求取最大值。

10.一种考虑时变时滞的移动机械臂避碰控制装置，其特征在于，包括下述模块：

指令接收模块，用于接收传感器发送的与移动机械臂相对应的原始控制指令；

模型获取模块，用于获取与移动机械臂相对应的基于时变时滞的状态模型；

时滞纠偏模块，用于根据估测算法对状态模型进行时滞纠偏操作，得到时变时滞信息；

控制器获取模块，用于获取与时变时滞信息相对应的模型预测控制器；

求解模块，用于根据模型预测控制器对原始控制指令进行求解操作，得到控制输入信号；

避碰轨迹计算模块，用于根据时变时滞信息以及控制输入信号计算与移动机械臂相对应的避碰轨迹；

操控模块，用于根据避碰轨迹对移动机械臂进行控制操作。

Images