仿生蛇形机器人主动避障的模型预测控制方法

技术领域

本发明涉及一种自动避障技术，特别涉及一种仿生蛇形机器人主动避障的模型预测控制方法。

背景技术

仿生蛇形机器人是一类具有细长、无肢身体的可移动型机器人。这类机器人通过自身的各个关节构成的驱动机构的变化，与环境相互作用，产生高效的移动能力。因其灵活、多自由度、多种运动形式等特点，蛇形机器人在各个邻域有着广泛的应用。例如：在崎岖复杂的地形中，蛇形机器人可以灵活地切换不同形式的运动步态以满足移动需求，帮助人们在无法到达的环境中作业；对于管道、溶洞等狭小的工作空间，蛇形机器人能够依靠自身细长的结构优势，代替人类完成探索和侦察任务；对于障碍物密集的场所，蛇形机器人可通过改变其自身形状，柔顺地避开障碍物或者借助障碍物产生移动推力。

在机器人的众多应用场景中，机器人如何实现无碰撞自主避障一直是人们关注的问题。蛇形机器人在运动中要适应各种复杂的环境，为了避免发生碰撞，运动控制器中需要考虑如何避开前进方向的障碍物。由于两种原因，蛇形机器人在期望路径中实现无碰撞避障面临巨大的挑战：首先，与传统的机器人相比，蛇形机器人没有固定的物理碰撞体积。机器人整个身体构成一个驱动器，通过拟合不同的曲线实现前进和转向运动，因此，在避障过程中需要考虑全部关节位置，这将加大控制器的设计难度。其次，由于其特殊的身体结构，蛇形机器人转向效率低下，与传统机器人相比，蛇形机器人的转向需要较大的转向空间，因此，在面对障碍物时，机器人需要提前预留较大的空间以满足无碰撞需求，预留空间的设计以及避开后何时回归期望路径是一个难点。

目前的机器人的避障任务分为两种：第一种是机器人需达到指定位置并根据障碍物位置提前规划好合适的绕障路线完成避障；第二种是机器人在跟踪指定路径中存在障碍物，之后根据障碍物的位置转向，在避开障碍物后回归到指定路径。针对蛇形机器人在期望路径上运动并完成避障任务的研究有很多。例如针对蛇形机器人在流场中各关节之间的轨迹跟踪问题，研究一种基于改进蛇形曲线的蛇形机器人在流场中避障的轨迹跟踪控制律，在流固耦合模型中加入势函数，使其避开障碍(基于改进蛇形曲线的蛇形机器人在流场中避障的轨迹跟踪控制律-李东方-2019)。

现有的提出了基于相位调整法和幅值调整法的蛇形机器人避障策略，通过调节蛇形机器人的相位实现转向，从而避开路径中障碍物(基于幅值调整法的蛇形机器人避障研究-卢振利-2016；基于相位调整法实现蛇形机器人避障功能的研究-卢振利-2015)。

现有的提出一种利用蛇形机器人避障方法，该方法利用头部关节参考角度相位调整法结合被动蜿蜒，通过能量状态调节头部关节输出力矩达到控制机器人躲避障碍物目的(蛇形机器人被动蜿蜒避障运动的研究-张丹凤-2011)。

1、现有方案中普遍基于头部的位置设计控制器，然而，机器人的身体并不总是通过机器人头部所遵循的路径，在避障过程中除了头部的其他关节部分可能会与障碍物发生碰撞。

2、现有方案面对障碍物时普遍只调整步态中各关节的相位偏差从而达到转向目的，而蛇形机器人转向效率低下，当转动角度较大时，转向空间也随之变大，这会牺牲跟踪期望路径的准确性并且产生不必要的能耗。

3、现有方案普遍面对障碍物时根据蛇头位置计算转向角，但机器人处于在周期性步态中的不同时刻时施加相同转向角的效果却大不相同，因此为了提高避障的精度，通常选择大角度转向，这会降低机器人能量的使用效率。

发明内容

针对仿生蛇形机器人在路径跟踪过程中避障的问题，提出了一种仿生蛇形机器人主动避障的模型预测控制方法。

本发明的技术方案为：一种仿生蛇形机器人主动避障的模型预测控制方法，建立蛇形机器数学模型，在蛇形机器数学模型基础上建立模型预测控制器，满足真实机器人的物理约束以及在推导蛇形机器人简化的平均系统过程中的关节角变化速度慢的假设条件下，以最小化跟踪误差和保证全身与障碍物无碰撞为目的，建立目标函数；蛇形机器人在执行跟踪有障碍物的路径任务中，当经过固定采样时间后，模型预测控制器根据当前时刻的状态量，利用模型预测控制器中预测模型预测时间段内机器人的状态，并根据速度和方向误差以及蛇身的每个连杆和障碍物间的距离求得一组最优解，即在预测时间段内的每一时刻最优的输入量；模型预测控制器只更新当前时刻所需最优的输入量，并保持到下一采样时刻的到来，重复上述优化操作，完成路径跟踪和避障操作。

进一步，所述蛇形机器数学模型建立方法：

1)蛇形机器人由n个长度为2l，质量为m，转动惯量为J＝ml

2)第一定律建立蛇形机器人的动力学模型如下：

式中f

M

S

3)假设作用在连杆i上的摩擦力[f

式中c

式中

4)利用平均角动量模型改写蛇形机器人数学模型：

由蛇形机器人关节角和连杆角的定义可得以下关系：

φ＝Dθ(5)

代入式(1)中可得：

这表示输入量u对于关节角φ是全驱动的，因此，在对于蛇形机器人位置p和方向

根据蛇形机器人的关节角和质心坐标的定义可得连杆位置z并求时间的微分，得到连杆的线性速度

为了从动力学模型(1)和(2)中推导出控制器设计所需的简化模型，引入一个新的变量对动力学模型进行改写：

改写后称之为平均角动量模型，对平均角动量模型(8)微分并代入式(1)中可以得到以下关系：

根据式(4)、式(5)、式(7)改写式(2)和式(9)得到

式(7)、式(10)和式(11)构成改写后蛇形机器人数学模型。

进一步，所述在蛇形机器数学模型基础上建立模型预测控制器，即利用平均化法对蛇形机器数学模型推导出模型预测控制器设计所需的简易模型，简化方法如下：

A：由于真实蛇形机器人的物理约束限制，模型预测控制器中添加有关节角变化限制，机器人关节角的幅值α(t)和偏移φ

φ

为了得到方向

(1)根据式(5)和式(8)可得关节角速度和连杆角速度的关系

式中

(2)根据式(5)和方向角的定义可得

式中Γ＝D

B：在使用平均化法简化模型前，先利用小角度近似法将部分模型线性化：假设关节角的角度很小，因此对于第i个关节，有

根据小角度近似法，可得新的定义：

根据式(14)、方向角

同时，对式(16)微分，并代入式(10)可得

定义蛇形机器人速度v中的前向速度v

将式(15)和式(18)代入式(7)、式(11)和式(17)可得

/>

利用小角度近似法对式(19)-(21)简化，得到如下三个等式：

C：利用平均化法从得到的式(22)、式(23)和式(24)推导出模型预测控制器设计所需的简易模型，对于如下系统

其中，函数f对t的周期为T，ε是一个很小的正参数，该系统的平均系统为

平均化法用系统(25)的平均系统的解逼近该系统的解，并且正参数ε越小，两系统的误差就越小；

由于在设计模型预测控制器时，添加了关节角的变化约束并且考虑到真实机器人的物理约束，因此连杆速度

式中T＝2π/ω，

/>

进一步，所述模型预测控制器建立及预测方法：

首先从平均系统(27)-(29)中得到以下状态空间方程：

其中状态方程的状态量

将状态空间方程(30)离散化，得到在t时刻用于模型预测控制器的预测模型：

式中σ表示时间步长，N

根据预测模型得到的预测范围内的预测状态

式中

蛇形机器人的参考速度由期望指标给出，同时机器人通过当前位置和期望路径得到参考角度，因此，在已知障碍物位置的情况下，机器人在路径跟踪过程中主动避障问题将转为在t时刻求解以下优化问题：

s.t.(31)(35)

其中

其中j＝0,1,...,N

本发明的有益效果在于：本发明仿生蛇形机器人主动避障的模型预测控制方法，运动控制器考虑蛇身全部连杆和障碍物的实际距离，避免全部关节与障碍物发生碰撞并且有效协调好稳定性与避障的行为冲突；实时调整蛇形机器人幅值大小，面对障碍物时通过缩减步态中的幅值大小减小所需转向角，从而降低不必要的能量损耗；小角度避障，减小转向空间并且在完成避障任务后能够快速回归期望路径中；在线优化策略，避障过程更加柔顺，避免突加转向角后转弯步态不连续、性能不稳定等问题。

附图说明

图1为本发明平面多连杆蛇形机器人运动学模型图；

图2为本发明模型预测控制器原理图；

图3为本发明蛇形机器人质心位置图；

图4为本发明障碍点局部放大图；

图5为本发明蛇形机器人关节的幅值和偏移量以及质心相对路径的偏差图。

具体实施方式

下面结合附图和具体实施例对本发明进行详细说明。本实施例以本发明技术方案为前提进行实施，给出了详细的实施方式和具体的操作过程，但本发明的保护范围不限于下述的实施例。

本发明针对机器人在跟踪指定路径中存在障碍物，之后根据障碍物的位置转向，在避开障碍物后回归到指定路径的任务需求，完成设计。基于模型预测控制策略，实现蛇形机器人无碰撞自主避开给定路径中障碍物。建立蛇形机器数学模型，在此基础上建立模型预测控制器，在满足真实机器人的物理约束以及在推导蛇形机器人简化的平均系统过程中的关节角变化速度慢的假设条件下，以最小化跟踪误差和保证全身与障碍物无碰撞为目的，建立目标函数；蛇形机器人在执行跟踪有障碍物的路径任务中，当经过固定采样时间后，模型预测控制器根据当前时刻的状态量，利用模型预测控制器中预测模型预测时间段内机器人的状态，并根据速度和方向误差以及蛇身的每个连杆和障碍物间的距离求得一组最优解，即在预测时间段内的每一时刻最优的输入量；模型预测控制器只更新当前时刻所需最优的输入量，并保持到下一采样时刻的到来，重复上述优化操作，完成路径跟踪和避障操作。

发明的具体步骤如下：

1、建立蛇形机器人的数学模型

蛇形机器人由n个长度为2l，质量为m，转动惯量为J＝ml

利用第一定律建立蛇形机器人的动力学模型如下

式中f

M

系数矩阵：V＝A

S

假设作用在连杆i上的摩擦力[f

式中c

/>

式中

z＝[x

2、利用平均角动量模型改写蛇形机器人数学模型：

由蛇形机器人关节角和连杆角的定义可得以下关系：

φ＝Dθ(5)

代入式(1)中可得：

这表示输入量u对于关节角φ是全驱动的。因此，在对于蛇形机器人位置p和方向

根据蛇形机器人的关节角和质心坐标的定义可得连杆位置z并求时间的微分，得到连杆的线性速度

为了从动力学模型(1)和(2)中推导出控制器设计所需的简化模型，引入一个新的变量对动力学模型进行改写：

可以称之为平均角动量模型。对平均角动量模型(8)微分并代入式(1)中可以得到以下关系：

根据式(4)、式(5)、式(7)改写式(2)和式(9)得到

为蛇形机器人控制器设计的简化模型将从式(7)、式(10)和式(11)中推导得到。

3、利用平均化法得到简化模型：

由于模型预测控制方法在求解优化问题时计算量大，因此需要对模型式(7)、式(10)和式(11)进行简化，避免过长的求解时间对控制器效果的影响。

根据蛇形机器人的正弦蜿蜒步态，蛇形机器人的关节角i满足以下关系

φ

其中α(t)和ω分别表示关节角i正弦运动的幅值和角频率，δ表示关节角i与相邻关节角的相位差。φ

φ

为了得到方向

(1)根据式(5)和式(8)可得关节角速度和连杆角速度的关系

/>

式中

(2)根据式(5)和方向角的定义可得

式中Γ＝D

在使用平均化法简化模型前，先利用小角度近似法将部分模型线性化：假设关节角的角度很小，因此对于第i个关节，有

根据小角度近似法，可得新的定义：

根据式(14)、方向角θ定义和小角度近似法得到的新的定义可得

同时，对式(16)微分，并代入式(10)可得

定义蛇形机器人速度v中的前向速度v

将式(15)和式(18)代入式(7)、式(11)和式(17)可得

利用小角度近似法对式(19)-(21)简化，得到如下三个等式：

/>

利用平均化法从得到的式(22)、式(23)和式(24)推导出控制器设计所需的简易模型。对于如下系统

其中，函数f对t的周期为T，ε是一个很小的正参数，该系统的平均系统为

平均化法用系统(25)的平均系统的解逼近该系统的解，并且正参数ε越小，两系统的误差就越小。由于在设计模型预测控制器时，添加了关节角的变化约束并且考虑到真实机器人的物理约束，因此连杆速度

式中T＝2π/ω，

Ψ

/>

4、设计模型预测控制器

蛇形机器人的模型预测控制器如图2所示。首先从平均系统(27)-(29)中得到以下状态空间方程：

其中状态方程的状态量

将状态空间方程(30)离散化，可以得到在t时刻用于模型预测控制器的预测模型：

式中σ表示时间步长，N

式中

蛇形机器人的参考速度由期望指标给出，同时机器人通过当前位置和期望路径可以得到参考角度，因此，在已知障碍物位置的情况下，机器人在路径跟踪过程中主动避障问题将转为在t时刻求解以下优化问题：

s.t.(31)(35)

其中

为了满足真实机器人的物理约束以及在推导蛇形机器人简化的平均系统过程中的关节角变化速度慢的假设，在求解优化问题(35)时，添加如下约束条件：

其中j＝0,1,...,N

蛇形机器人在执行跟踪有障碍物的路径任务中，当经过固定采样时间T

通过调节距离代价和误差代价的权值可以调节任务优先级，同时距离代价函数可以满足当障碍物距离较远时，距离代价函数数值较小，任务优先级远小于误差代价的优先级，保证跟踪路径的准确性，当接近障碍物时，距离代价函数数值较大，任务优先级大于误差代价的优先级，避免机器人与障碍物发生碰撞，合理的将两种任务结合在一起。

5、仿真

通过MATLAB对基于模型预测控制策略实现蛇形机器人无碰撞自主避开给定路径中障碍物进行仿真验证，蛇形机器人的质心位置如图3所示，蛇形机器人所有连杆经过区域在障碍物处的局部放大图如图4所示，蛇形机器人的控制器输出量和质心位置相对于路径的偏差如图5所示。根据仿真结果可以说明基于模型预测控制的蛇形机器人自主避障算法可以有效使所有连杆无碰撞避开障碍点，同时也不会破坏跟踪路径的准确性。根据控制器的输出量可以说明当机器人面对障碍物时通过降低步态中的幅值大小，从而减小所需关节偏移角φ

以上所述实施例仅表达了本发明的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进，这些都属于本发明的保护范围。因此，本发明专利的保护范围应以所附权利要求为准。