轮手一体机器人能量次优重构规划方法

胡亚南 马书根 李斌 王明辉 王越超

轮手一体机器人能量次优重构规划方法

胡亚南1,2马书根1,3李斌1王明辉1王越超1

模块化机器人的重构规划中,由于各模块的目标分配与其轨迹规划之间的耦合关系导致组合爆炸问题.本文提出一种基于简化模型的能量次优规划方法,将重构规划问题转化为最优控制问题,实现目标分配与轨迹规划的解耦.通过求解由Hamilton-Jacobi-Bellman(HJB)方程描述的最优控制问题,得到简化模型的值函数和最优轨迹.各模块的运动目标由值函数的吸引域决定.通过在最优轨迹附近的次优区域内搜索得到实际运动轨迹,提高了搜索效率.仿真实验结果表明,该方法能够选择合适的模块组合,并能在障碍物环境中生成满足机器人动力学约束的运动轨迹.

模块化机器人,重构规划,模型简化,最优控制

在灾难救援、野外侦查等任务中,机器人面对的是不确定的多样化环境.模块化机器人能够组成具有不同运动性能的构形以满足不同的任务或环境要求,这一过程又称为重构.根据几何结构,模块化机器人可分为三类:栅格型、链型和移动型[1].轮手一体机器人是一种移动型模块化机器人,其单模块由五自由度机械臂和履带轮体组成[2],如图1(a)所示.在移动时,机械臂用于改变方向;在重构时,其负责连接各模块组成群体构形,如图1(b)所示.

重构规划的任务是选择合适的模块并生成其运动轨迹从而组成特定的构形.根据初始状态的不同,重构可分为由多个独立的模块组合成群体构形,以及从一种构形变换为另一种构形,如图1(c)所示.如果不考虑机械臂最后的对接步骤,可以将后者视为前者的特例.本文只考虑一般情况,即独立模块组成群体构形.与传统运动规划相比,重构规划面临两个挑战.首先,每个模块作为一个独立运动的机器人,都需要对其运动进行规划,这产生高维的构型空间.而规划算法的复杂度与空间维度呈指数关系.其次,在规划中不仅需要考虑模块自身以及模块间的约束关系,还要考虑当前模块对整体结构可能的依赖和影响.为了避免对大量模块同时进行规划,文献[3−4]采用宏模块的思想,将部分模块作为一个整体进行规划.文献[5−6]提出基于规则的策略以处理模块间复杂的约束,减少搜索空间.现有的重构规划研究主要针对栅格型和链型模块化机器人,其特点是单个模块需借助其他模块辅助运动才能实现重构.而且,现有方法只考虑了简单的几何约束或运动学模型,忽略了机器人的完整(动力学)模型,对重构策略的最优性也缺少讨论[1].文献[7]将模块状态的欧氏度量作为优化指标,并使用分配算法得到最优的重构方案.但存在运动学约束时,欧氏度量并不准确,而且协调机器人间的冲突时所采用的反应式方法,只能保证运动是局部最优而不能保证全局最优.

图1 轮手一体机器人及其重构Fig.1 Wheel-manipulator robots and their recon fi guration

传统单机器人运动规划中考虑动力学约束的主要有三种方法:1)基于采样的方法[8]通过对状态空间或控制空间进行采样,并对动力学方程积分生成可行轨迹.通过建立并逐渐扩展树形结构实现对状态空间的探索.该方法的优点是计算简单,缺点是生成的轨迹通常远离最优解,一般需要后期优化;2)基于分层的方法[9]将规划分为两层实现,高层负责生成一系列局部目标,其中只考虑机器人的运动学约束和几何约束,而低层负责搜索连接局部目标的可行轨迹,其中考虑动力学约束.通过分层限制了探索区域,从而提高了效率.但高层在考虑运动学约束时对控制量取极值,只适用于机器人速度幅值不受约束时距离最优的情况.而在速度幅值受约束并以能量为优化准则的情况下,机器人的转向半径并非极值[10];3)基于数值优化的方法[11]将机器人的控制量用参数表示,使用数值优化方法得到控制参数值和机器人的轨迹.其优点是通用性好,适用于任意模型、优化目标和约束条件,缺点是计算量较大且容易收敛到局部极小值.

移动型模块化机器人的重构规划中,目标构形通常是给定的,而各初始模块对应的子目标(即目标构形中的模块)是未知的.初始模块与子目标间存在多种分配方式.为了实现重构的总代价最优,对各模块进行子目标分配时需要根据各自的运动轨迹计算其代价,而规划运动轨迹的前提是已知子目标,二者存在耦合关系.传统方法只从单模块角度考虑,穷举出所有的可能的组合并选择总代价最小的一个.其缺点是:组合方式的数量与机器人模块数量呈阶乘关系,计算量随模块增加而急剧增加;而且没有从整体考虑各模块之间的运动影响,各模块由于运动存在约束,孤立地对各模块进行规划所得到的轨迹会出现干涉从而导致规划的轨迹不可行.

1 基于简化模型的重构规划方法概述

本节通过一个例子阐述所提方法的基本思想.考虑二维平面中运动不受约束的机器人重构问题,如图2(a)所示.目标构形由两个模块组成,但环境中共有3个独立模块.任务是选择两个模块使其到达子目标,并且两个模块的移动距离之和最小.为了避免子目标分配与单模块运动规划之间的耦合,规划运动轨迹时,不是从初始模块开始,而是反其道而行之,以子目标为初始值,根据动态规划的思想迭代计算平面中各点到(任一)子目标的距离的最小值.最终得到以各点坐标为自变量的函数,称为值函数.值函数的图像为如图2(b)所示的双圆锥,锥尖对应子目标所在的位置.各模块沿着锥面做梯度下降即可得到最优轨迹,因为梯度方向是距离下降最快的方向.对每个子目标,选择最先到达该子目标的模块就是满足要求的重构组合.值函数的梯度定义了一个最优反馈向量场,如图2(c)所示,对位于任意初始位置的模块,这一向量场给出了到达子目标的最优控制律.值函数对于每个子目标构成了一个吸引域,不同吸引域的边界就是值函数梯度为0的区域,如图2(c)中的虚线所示.

值函数可以作为Lyapunov函数,设计稳定的控制器,也可类比为人工势场法中的势函数,其作为导航函数的一个特例,常用于设计反馈运动规划策略.例如文献[12]通过构造一系列的局部反馈控制器(被形象地比喻为漏斗)将障碍物环境中的复杂控制系统稳定到目标区域.但人工势场法中对势函数的定义只考虑局部信息,导致势函数中存在局部极小值,从而使机器人无法到达目标点.而值函数不存在局部极小值.此外,势函数中的吸引势场采用启发式的方法计算得到,其是对机器人当前状态到目标点真实代价的下界估计值.而值函数自身反映了机器人到目标点的真实代价信息.借助这一特点,可以将其由单机器人单目标的运动规划推广到多机器人多目标的运动规划,例如重构规划.

图2 二维无约束机器人重构例子Fig.2 The recon fi guration example of unconstrained robots in two dimensions

上述重构规划步骤可以总结为:

步骤1.确定优化准则,以子目标为初始值根据机器人模型求解其最优控制问题,得到值函数和最优反馈控制律.

步骤2.选取值函数最小的模块,使用最优控制律对机器人模型积分得到不同初始状态下的最优轨迹,完成重构.

如果将以上方法直接用于轮手一体机器人重构规划中,仍会面临两个问题:首先,机器人动力学模型维数较高且非常复杂,直接求其最优控制问题非常困难;其次,由于机器人受动力学约束,得到的最优轨迹间可能存在干涉.

在结构力学、流体力学等领域,描述系统的模型往往也表现出高维、复杂的特征.但通过分析可以发现,并非所有状态变量对系统都有同等程度的影响.系统的行为可以通过几个主要维度近似刻画,而忽略次要维度所引起的误差可以控制在一定的范围内.因此可以使用简化的模型近似描述复杂系统的行为.在控制理论等领域中,如果描述系统的模型维数过高或规模较大时,人们也会寻找一个规模较小的简化模型来逼近完整的系统模型,这被称为模型降阶.类似思想在模型较复杂的机器人研究中得到体现,例如用倒立摆模型为人形机器人设计控制律.现有的模型降阶方法主要针对线性系统[13].对于非线性系统需要预先在选定的名义状态附近进行近似线性处理[14],导致其只能适用于名义状态的临域,并且在不同的名义状态下需要切换.早期针对非线性系统的约简(降阶)通常采取精确的方式,其依赖系统的某些特性,例如对称性.因此精确约简一般用于满足动量守恒的系统或受非完整约束的系统[15],这限制了其适用范围.为了处理更一般的动力系统,有学者提出近似约简的思想[16],其适用于具有一定稳定性的系统.受到上述研究的启发,本文首先对机器人的运动模型进行分析,得出其在最优控制律作用下的稳定特性,并基于近似约简思想对机器人模型进行简化处理.然后通过求解简化模型的最优控制问题,得到其最优轨迹.随后最优轨迹附近搜索得到满足机器人动力学约束的能量次优轨迹.

2 机器人运动模型的分析与简化

本节对机器人的运动模型进行分析并给出其简化模型.在此之前,首先介绍后文用到的数学知识.

2.1 基本数学知识

向量场表示为(Rn,X),其中X 是光滑映射X:Rn→ Rm.如果光滑曲线x:I→ Rn满足x(t)=X(x(t)),∀t∈I(I是R 的开子集),则称x:I→Rn是向量场(Rn,X)的轨迹.为了强调轨迹的初始条件x(0)=x0,通常将轨迹表示为x(·,x0).控制系统表示为(Rn,Rm,F).其中F 是光滑映射F:Rn×Rm→Rn.假设系统的状态空间是Rn,对于它的一个分解Rn=Rm×Rk,可以定义投影πm:Rn→Rm和πk:Rn→Rk.

定义1.向量场(Rn,X)与(Rm,Y)被称为近似πm相关[16],如果存在K∞类函数γ和常数c∈R+0使得对任意的初始状态x∈Rn和时间t∈,有 |πm◦x(t,x)− y(t,πm(x))|＜ γ(|πk(x)|)+c.其中,|x|表示状态x的欧氏范数.系统完整模型对应的向量场与其简化模型对应的向量场近似相关意味着,完整模型轨迹的投影与简化模型的轨迹是有界的,其边界是初始状态的函数.

定义2.控制系统(Rn,Rm,F)被称为增量一致有界输入有界状态稳定(Incrementally uniformly bounded input bounded state stable,IUBIBSS),如果存在K∞类函数γ1和γ2以及常数d∈使得对所有t∈,初始状态x1,x2∈Rn和控制输入其中,k f k=esssupt∈[0,τ]|f(t)|, 对于任意函数f:[0,τ]→ Rn,τ∈ R+.文献[16]给出了两个向量场近似相关的充分条件:

定理1.对于向量场(Rn,X),定义F=Tπm·X:Rm×Rk→Rm是状态空间Rm上的控制系统.向量场(Rm,Y)定义为Y(y)=T(y,0)πm·X(y,0)=F(y,0).如果(Rn,X)是沿纤维实际稳定的(Fiberwise practically stable),且F 满足IUBIBSS,则向量场(Rm,Y)和(Rn,X)是近似πm相关的.

2.2 机器人运动模型分析

下面介绍轮手一体机器人的运动模型.单模块机器人的构型可由描述.其中,矩阵表示轮体坐标系Obxbybzb相对于全局坐标系OIxIyIzI的位姿,而向量 q 表示手臂关节转角.位姿矩阵 gggIb中的姿态存在多种表示方法,本文选取欧拉角参数表示.因此,单模块机器人的构型坐标为,其中x,y和z为轮体质心坐标,而θ,φ和ψ为轮体坐标系的XY Z欧拉角.轮手一体机器人的动力学模型[17]为

将式(1)和式(2)合写为状态空间的形式

最优控制问题的代价泛函定义为状态和控制的二次型函数

下面对各状态在式(3)中的影响程度进行分析.在正常行进时,机器人轮体的x,y和θ坐标可以主动控制,而φ,ψ和z则由地形约束决定,是被动量.在地形起伏较小的环境中,ωx,ωy和vz的变化较小.轮手一体机器人采用履带推进,在不打滑的情况下,轮体侧向滑移量很小,因此侧向速度vy对整体运动的贡献较小.机器人在转向时,转向速度ωz较小,手臂的关节变化范围有限且速率较小.

基于以上分析,将机器人的状态分解为主要分量和次要分量,即.其中主要分量由变化较大的状态变量组成,即. 并由此定义投影πs:.根据状态的分解,式(4)可以写成

由于未知参数和无法建模的因素,模型与实际系统总存在误差.而且机器人在实际运动时可能受到外部扰动.因此,在最优控制的基础上考虑一定的鲁棒控制更有实际意义.根据文献[18]的结论,系统在代价下的最优控制也是系统(4)的鲁棒控制.通过求的最优控制问题,可以得到最优反馈控制律.

证明.定义代价泛函

然后分析系统F=Tπs·f的稳定性质.验证F的稳定性同样需要构造相应的Lyapunov函数.由于系统(8)渐近稳定,因此满足部分稳定(Partial stability).根据Lyapunov逆定理[19],对于不同的主要状态和次要状态(此时视为输入),存在函数满足,且有和.定义.所以可以作为 F 的 IUBIBSS Lyapunov函数,因此F满足IUBIBSS.

图3 完整模型与简化模型的运动轨迹Fig.3 The trajectory of the full model and that of the simpli fi ed model

这一结论的意义在于,通过求解相对简单的简化模型对应的最优控制问题,可以得到近似的值函数,由此可得到次优的子目标分配.而且可以在简化模型对应的最优轨迹附近搜索得到满足完整模型约束的次优轨迹,从而提高规划的效率.为此需要得到简化模型的具体形式.

2.3 简化模型的形式

其中,uθ表示转向控制量,uv是履带转动产生的加速度.为方便求解,将式(9)改写为以下形式

其中,v表示轮体的前进速度vx,a是轮体驱动器导致的加速度,ae是外力造成的加速度,例如重力,s表示机器人转向控制量.简化模型的状态为,控制量为=(a,s).

3 简化模型的最优控制

得到轮手一体机器人的简化模型后,下面建立并求解其最优控制问题.

3.1 最优控制问题描述

动态规划原理给出了最优控制问题的充分条件,对于连续时间问题,可由其推导出Hamilton-Jacobi-Bellman(HJB)方程.通过求解HJB方程可以得到简化模型的最优控制律以及值函数.受体积限制,模块化机器人携带的能源有限,因此本文选取能量作为优化目标.轮手一体机器人采用直流电机驱动,根据文献[10],电机消耗的能量可以表示为速度和加速度的函数

其中,c1,c2,c3和c4均为反应机器人和地形特性的常数.机器人的控制量受到环境和自身的约束,因此需要确定控制量的可行范围.

首先,机器人的加(减)速度受电机最大输出力矩的限制,应该位于最大加(减)速度范围内,即amin≤a≤amax.机器人的转向半径r=1/|s|,其不应小于最小转向半径rm,所以对转向控制量的约束为|s|≤1/rm.此外,考虑到机器人的转弯速度应受到地面约束力的限制,其转弯的离心力不应超过地面能提供的最大摩擦力Fmax,即

从而得到另一约束关系|s|≤Fmax/(mv2).因此,定义可行控制空间为

基于动态规划原理建立简化模型(10)的HJB方程

其中,V是值函数,∇V是V的梯度(后文用下标表示V 在某方向的偏导,例如Vx=∂V/∂x),“·”表示内积.注意式(14)左侧的负号会影响其粘性解,不可省略.将式(10)和(11)代入式(14)展开后得到:

3.2 HJB方程数值求解

HJB方程是一阶双曲型偏微分方程,其经典意义上的解(即连续可微解)不存在,所以一般寻求其弱解,其中最常用的是粘性解[20].由于系统模型的非线性特征,难以得到HJB方程(15)的解析解,因此本文采用数值方法.根据文献[21]的结论,单调的差分格式能够收敛到正确的粘性解,本文采用Godunov差分方法[22].将简化模型的状态空间离散化,建立分辨率为(hx,hy,hθ,hv)的网格,随后的计算针对网格的节点.令Vi,j,k,w=V(ihx,jhy,khθ,whv)表示值函数在节点坐标(i,j,k,w)处的解.下面按照Godunov格式逐维对方程(15)离散化.首先,式(15)中不含有控制量的项可以直接提出

式(16)在x方向的离散结果为

其中,ξx=|v cosθ|.

y方向的离散结果为

其中,ξy=|v sinθ|.

θ方向的离散结果为

有学者对于中药重金属污染情况特别关注，并进行了系统的考察评价。赵连华等［8］得出这5种重金属的污染率在9.33%～26.35%，并且因为产地不同污染情况不同。郭兰萍等［9］以《中医药——中药材重金属限量》ISO国际标准为依据，分析了中药材中Pb、As、Cd、Hg 4种重金属元素的污染情况，结果显示4种重金属的超标率分别为3.46%，4.03%，2.91%，1.41%。而重金属元素一旦进入人体后，由于其半衰期较长，在人体内的含量不断增高后，会诱发人体内的各种疾病［10］。

根据Godunov方案,|Vθ|的离散结果为

为使(19)中的最小值符号成立,最优控制s的符号应和Vθ的符号相反,幅值应为中允许的最大幅值.

v方向记为

式(21)的最小值符号中的项是a的二次函数,将其记为fa(a).其最小值分为以下三种情况:

1)−Vv/2c1＞ amax,此时函数最小值取在a=amax处,最小值为.

2)−Vv/2c1＜ amin,此时函数最小值取在a=amin处,最小值为.

3)amin＜−Vv/2c1＜amax,此时函数最小值取在对称轴a=−Vv/2c1处,最小值为.

对v方向离散时需要根据上述三种情况分别讨论:

1)只需离散方程右侧的第一项,即

2)与情况1类似,有

其中,ζ2=sgn(ae+amin).

3)此时将fa视为Vv的函数,其最值取在

将各维状态的离散结果代入式(16)中,得到Vi,j,k,w与其邻近节点值的方程,记为

这样就得到了一组非线性代数方程,方程的个数等于网格节点数.求解大规模的代数方程,需要高效的数值方法.本文采用快速扫描法(Fast sweeping method)[23−24],其使用了迭代思想,收敛速度快,计算复杂度为O(N),其中N 为网格节点数量.求解偏微分方程需要给定边界条件.目标构形的状态是给定的,在计算时,将目标构形中各模块的主状态分量所在的节点的节点值Vi,j,k,w设为0.在最优控制问题的框架下,还可以考虑障碍物和环境地形等约束.在对网格节点初始化时,障碍物所在的区域内的节点值被设置为较大的数(大于所有轨迹的最大能量即可),在随后扫描时其值不被更新.机器人在不平整地形中运动时,所受的约束与地形有关.假设地形由光滑函数z=M(x,y)定义,如图4所示.

图4 不平整地形中的运动轨迹参数Fig.4 The parameters of the trajectory on uneven terrain

机器人运动轨迹上某点处的单位切向量为

单位法向量为

考察机器人轨迹在地形切平面内的曲率,即测地线曲率

其中,“×”表示差积.轨迹的曲率应满足:

将式(26)～(28)代入式(29)可求得控制量s的约束

其中,Mxy= ∂2M/(∂x∂y).

求解得到了状态空间中各个节点处的离散最优控制律,如果想要得到非节点处的控制律则需要对离散控制律插值.根据最优控制律,以各模块初始状态为初始值,对简化模型(10)数值积分即可得到最优轨迹.对如图5所示的存在障碍物和地面不平坦的环境,应用上述方法计算不同初始位姿到目标状态的最优轨迹.箭头方向表示机器人的初始偏航角θ.惟一的目标点位于环境中心.优化目标的参数为c1=c2=c3=0,c4=1.

图5 不同初始位姿及单目标下简化模型的最优轨迹Fig.5 The optimal trajectories of the simpli fi ed model with a single goal and di ff erent initial poses

4 轨迹规划

在各模块子目标已知的基础上,本节提出满足机器人实际动力学约束的轨迹规划方法.这一过程需要动力学模型的仿真,为此首先介绍机器人的受力模型,然后给出轨迹搜索方法.

4.1 轮手一体机器人受力模型

精确的模型可以准确地预测机器人的运动,从而提高轨迹跟踪效率.机器人模型除自身动力学模型外,其在环境中的受力模型对其运动的预测同样重要.因此,下面给出机器人的受力模型.

轮手一体机器人轮体的履带提供主要推进力,可以用地面力学中的履带受力模型描述.为了进行仿真,需要对其离散处理.本文将履带与地面接触的一面均匀划分为若干离散单元,每个单元与地面视为点接触[25],从而将连续的分布力用多个离散力近似表示,如图6所示.每个接触点的受力由三个分量组成:法向力Fn,ij、切向力Ft,ij和侧向力Fl,ij.第ij单元的面积为ΔAij=ΔxΔy.在第j列单元(沿xb方向)设立坐标系Ojxjyjzj,各单元中心坐标记为xiyj.

图6 履带接地面作用力离散示意图Fig.6 Illustration of the discretized track-terrain forces

首先建立法向力的模型,t时刻xiyj坐标处的法向应力为

其中,kc,kφ是反应地面特征的常数,wt是履带的宽度,Δzij是xiyj坐标处履带的沉陷深度,nt为沉陷指数,C为阻尼系数.

假设ij单元内的应力分布均匀,同为σ(xiyj,t).xiyj坐标处的法向力代表其所在单元内的法向合力,即应力与单元面积的乘积

履带的切向应变jx(xiyj,t)也是位置和时间的函数,其满足如下微分方程

其中,rw为履带驱动轮的半径,ωw为驱动轮驱动电机的转速,也是轮手一体机器人的实际控制量.

解出jx(xiyj,t)后可计算xiyj处的切向应力

其中,c,ϕ和K 也是由实验测得的反应地面特征的常数.

根据切向应力可计算xiyj坐标处的切向力

侧向力的计算方法与切向力类似

为将上述力分量引入机器人的动力学模型,需将其表示为力旋量,并借助雅克比矩阵将其转化为广义力.

4.2 轨迹搜索方法

为了提高轨迹规划的效率,可以利用完整模型最优轨迹的投影与简化模型的最优轨迹有界这一信息,将简化模型的轨迹作为基准,在其附近搜索满足实际约束的可行轨迹.虽然完整模型与简化模型的轨迹偏差是有界的,但难以估计其边界.定义状态空间中到简化模型的最优轨迹的距离小于ε的区域为ε-次优区域.采用的ε越大,搜索的轨迹越可能接近最优,但计算量也越大.在实际搜索时,可以根据要求调整ε的大小.在对轨迹质量要求高且时间宽裕时,设置较大的ε;在对轨迹质量要求低且计算时间有限时,设置较小的ε.本文采用Dijkstra图搜索方法,从各模块的起始状态开始构造数棵树,树的节点是各模块的状态,树的边是各模块的可行轨迹.

为了防止搜索在某个局部区域内过度探索,将次优区域内状态空间的子集Q′=(x,y,θ)划分成网格,并规定每个网格内最多只能有一个状态节点.搜索的具体步骤如下:

步骤1.将各模块的起始状态作为树的根节点,确定半径ε.

步骤2.根据Dijkstra算法计算需要扩展的节点,对各模块的控制量采样并代入动力学模型(1)依次进行动力学仿真,规定机器人的移动距离到达一定距离Δs后停止.采样策略为:在以最优控制为中心的椭圆区域内进行采样.得到控制量后再变换到机器人的实际输入控制量,变换关系为

其中,函数PI是比例积分控制器,用于控制机器人的电机转速ωw以跟踪期望的加速度ar,函数InverseKinematics根据手臂导轮的偏角通过逆运动学计算得到手臂关节角度.

步骤3.判断仿真得到的轨迹是否可行,如果可行则加入树中.判断依据为

1)轨迹位于次优区域内;

2)机器人模块保持稳定,没有倾翻;

3)机器人模块间的距离大于安全阈值;

4)机器人模块与障碍物的距离大于安全阈值;

5)各轨迹的末端所在的网格内无其他轨迹的状态节点.

步骤4.判断各模块是否到达子目标.计算模块与子目标的距离,距离小于规定阈值则视为到达.否则返回步骤2.

5 仿真验证

本节通过仿真实例验证所提出的重构规划方法的有效性.基于符号计算软件Mathematica搭建了运动规划平台,可用于动力学积分,轨迹搜索和结果的动画显示.为提高计算效率,求解HJB方程的快速扫描法用C++语言实现,计算结果被Mathematica调用.仿真使用的参数见表1,仿真环境配置为Intel Core i5 6400,2.7GHz CPU,8GB RAM.

表1 仿真参数Table 1 The simulation parameters

实例1的目标构形为三角构形,其位置在原点.环境中存在4个障碍物和三个独立的模块,模块的初始位姿由随机生成.HJB方程的离散网格规模为70×70×50×15(分别对应x,y,θ,v维度),求解HJB方程所需时间为386s(迭代误差小于0.1视为满足收敛要求).重构规划的结果如图7(a)所示.实线是搜素得到的各模块的实际运动轨迹,而虚线是简化模型的最优轨迹,各模块均避开了静态障碍物并到达目标构形中相应的子目标,而且轨迹之间无干涉.图7(b)为各模块轮体电机的转速.从图7中可以发现,各模块基本都先以较大的加速度达到最大速度,在接近目标时又以最大能力减速,其变化符合Bang-bang控制的特征.

图7 三角构形重构例子Fig.7 The recon fi guration example of the triangle con fi guration

实例2的目标构形为串形构形,其由3个模块组成,附近存在5个模块,其位姿为随机生成.由于组成串形构形的相邻模块的间距较小,各运动模块在接近目标的过程中为避免碰到已就位的其他模块需要调整手臂的姿态,这会使其难以到达各自的目标.为此,将串形构形进行纵向的分解,以增大模块的间距,待各模块就位后再作简单的平移组成最终的构形.为实现重构,显然只需要三个独立模块.以独立模块为出发点的简化模型轨迹如图8(a)所示.其中,模块3和模块5到达同一子目标,模块1和模块4到达同一子目标.从模块1～5出发的简化模型轨迹对应的能量分别为:291.1、168.7、308.0、113.8和199.4.根据能量最小的准则,模块2、模块4和模块5被选择参与重构.重构过程中各模块的运动状态如图8(b)所示.

在仿真中,出现了有些子目标的吸引域内无模块的情况,此时无法顺利完成重构,如图9(a)所示的例子.进一步分析发现,值函数吸引域的边界(梯度为0的区域)形成Voronoi图,如图9(b)所示的三个子目标的情况,同心圆表示值函数的等值线.能量最优准则下得到的值函数吸引域边界则是以能量为度量的状态空间中的广义Voronoi图.如果出现无法重构的情况,可根据模块所处的状态采取不同的措施:在模块距离吸引域的边界较近时,可对其施加轻微扰动,使其落到期望的子目标的吸引域中;在模块距离吸引域的边界较远时,可以通过设置虚拟障碍物的方式对最优控制的计算结果进行干预,使得值函数形成的吸引域符合最优的分配.

图9 二维无约束机器人距离最优重构的值函数Fig.9 The value function of the recon fi guration of two dimensional unconstrained robots with optimal distance

6 结论

本文针对移动型模块化机器人的重构规划问题提出一种能量次优的规划方法.借鉴近似约简思想,得到机器人的简化模型,通过求解最优控制问题,解决了目标分配导致的组合爆炸问题.在轨迹规划中将搜索范围集中于次优区域,提高了算法效率.仿真结果表明,本文方法能够在有障碍物的三维地形中选择合适的模块及其对应的目标,并得到满足机器人动力学约束的运动轨迹.

附录

其中,cθ=cosθ,sθ=sinθ,余下同理.

1 Yim M,Shen W M,Salemi B,Rus D,Moll M,Lipson H,Klavins E,Chirikjian G S.Modular self-recon fi gurable robot systems.IEEE Robotics&Automation Magazine,2007,14(1):43−52

2 He Xin-Yuan,Ma Shu-Gen,Li Bin,Wang Yue-Chao.Mechanical design of recon fi gurable planetary rover.Chinese Journal of Mechanical Engineering,2005,41(12):190−195

(贺鑫元,马书根,李斌,王越超.可重构星球探测机器人的机构设计.机械工程学报,2005,41(12):190−195)

3 Bhat P,Ku ff ner J,Goldstein S,Srinivasa S.Hierarchical motion planning for self-recon fi gurable modular robots.In:Proceedings of the 2006 IEEE/RSJ International Conference on Intelligent Robots and Systems.Beijing,China:IEEE,2006.886−891

4 Dewey D J,Ashley-Rollman M P,De Rosa M,Goldstein S C,Mowry T C,Srinivasa S S,Pillai P,Campbell J.Generalizing metamodules to simplify planning in modular robotic systems.In:Proceedings of the 2008 IEEE/RSJ International Conference on Intelligent Robots and Systems.Nice,France:IEEE,2008.1338−1345

5 Vassilvitskii S,Kubica J,Rie ff el E,Suh J,Yim M.On the general recon fi guration problem for expanding cube style modular robots.In:Proceedings of the 2002 IEEE International Conference on Robotics and Automation.Washington,USA:IEEE,2002.801−808

6 Yoshida E,Murata S,Kamimura A,Tomita K,Kurokawa H,KokajiS.A motionplanningmethodforaselfrecon fi gurable modular robot.In:Proceedings of the 2001 IEEE/RSJ International Conference on Intelligent Robots and Systems.Maui,HI,USA:IEEE,2001.590−597

7 Hu Y N,Ma S G,Li B,Wang M H,Wang Y C.Recon fi guration planning for wheel-manipulator robots.In:Proceedings of the 5th Annual IEEE International Conference on Cyber Technology in Automation,Control and Intelligent Systems.Shenyang,China:IEEE,2015.529−534

8 Lavalle S M,Ku ff ner J J Jr.Randomized kinodynamic planning.The International Journal of Robotics Research,2001,20(5):378−400

9 Cherif M.Motion planning for all-terrain vehicles:a physical modeling approach for coping with dynamic and contact interaction constraints.IEEE Transactions on Robotics and Automation,1999,15(2):202−218

10 Tokekar P,Karnad N,Isler V.Energy-optimal trajectory planning for car-like robots.Autonomous Robots,2014,37(1):279−300

11 Howard T M,Kelly A.Optimal rough terrain trajectory generation for wheeled mobile robots.The International Journal of Robotics Research,2007,26(2):141−166

12 Burridge R R,Rizzi A A,Koditschek D E.Sequential composition of dynamically dexterous robot behaviors.The International Journal of Robotics Research,1999,18(6):534−555

13 Zhu Yao-Lin,Yang Zhi-Hai,Chen Xi-Hao.Investigation on the methods of model reduction.Microcomputer Information,2011,27(6):22−25

(朱耀麟,杨志海,陈西豪.模型降阶方法研究.微计算机信息,2011,27(6):22−25)

14 Yamane K.Systematic derivation of simpli fi ed dynamics for humanoid robots.In:Proceedings of the 12th IEEE-RAS International Conference on Humanoid Robots.Osaka,Japan:IEEE,2012.28−35

15 Ostrowski J P.Computing reduced equations for robotic systems with constraints and symmetries.IEEE Transactions on Robotics and Automation,1999,15(1):111−123

16 Tabuada P,Ames A D,Julius A,Pappas G J.Approximate reduction of dynamic systems.Systems&Control Letters,2008,57(7):538−545

17 Hu Ya-Nan,Ma Shu-Gen,Li Bin,Wang Ming-Hui,Wang Yue-Chao.Modular dynamics modeling approach of group con fi guration of wheel-manipulator robots.Journal of Mechanical Engineering,2015,51(1):24−33

(胡亚南,马书根,李斌,王明辉,王越超.轮手一体机器人群体构形的模块化动力学建模方法.机械工程学报,2015,51(1):24−33)

18 Lin F,Brandt R D,Sun J.Robust control of nonlinear systems:compensating for uncertainty.International Journal of Control,1992,56(6):1453−1459

19 Khalil H K.Nonlinear Systems(3rd Edition).Beijing:Publishing House of Electronics Industry,2012.116−120(Khalil H K.非线性系统(第3版).北京:电子工业出版社,2012.116−120)

20 Bardi M,Capuzzo-Dolcetta I.Optimal Control and Viscosity Solutions of Hamilton-Jacobi-Bellman Equations.Boston:Birkh¨auser,1997.

21 Crandall M G,Lions P L.Two approximations of solutions of Hamilton-Jacobi equations.Mathematics of Computation,1984,43:1−19

22 Osher S,Fedkiw R.Level Set Methods and Dynamic Implicit Surfaces(Applied Mathematical Sciences).New York:Springer,2003.

23 Tsai Y H R,Cheng L T,Osher S,Zhao H K.Fast sweeping algorithms for a class of Hamilton-Jacobi equations.SIAM Journal on Numerical Analysis,2003,41(2):673−694

24 Takei R,Tsai R.Optimal trajectories of curvature constrained motion in the Hamilton-Jacobi formulation.Journal of Scienti fi c Computing,2013,54(2−3):622−644

25 Solis J M,Longoria R G.Modeling track-terrain interaction for transient robotic vehicle maneuvers.Journal of Terramechanics,2008,45(3):65−78

26 Huang M H,Orin D E.Dynamic simulation of activelycoordinated wheeled vehicle systems on uneven terrain.In:Proceedings of the 2001 IEEE International Conference on Robotics and Automation.Seoul,South Korea:IEEE,2001.779−786

胡亚南 中国科学院沈阳自动化研究所博士.主要研究方向为机器人运动规划.本文通信作者.

E-mail:robinvista2@gmail.com

(HU Ya-Nan Ph.D.candidate at Shenyang Institute of Automation,Chinese Academy of Sciences.His main research interest is robot motion planning.Corresponding author of this paper.)

马书根 日本立命馆大学机器人系教授,中国科学院沈阳自动化研究所研究员.主要研究方向为仿生机器人,防灾救援机器人,环境适应机构学.

E-mail:shugen@se.ritsumei.ac.jp

(MA Shu-Gen Professor in the DepartmentofRobotics,Ritsumeikan University,Japan.He is also a professor at Shenyang Institute of Automation,Chinese Academy of Sciences,China.His research interest covers biomimetic robots,rescue robots,and environment-adaptive mechanism.)

李 斌 中国科学院沈阳自动化研究所研究员.主要研究方向为仿生机器人,移动机器人,机器人控制.

E-mail:libin@sia.cn

(LI Bin Professor at Shenyang Institute of Automation,Chinese Academy of Sciences.His research interest covers biomimetic robots,mobile robots,and robot control.)

王明辉 中国科学院沈阳自动化研究所研究员.主要研究方向为移动机器人,机器人控制,多机器人协作.

E-mail:mhwang@sia.cn

(WANG Ming-HuiProfessorat Shenyang Institute of Automation,Chinese Academy of Sciences. His research interest covers mobile robots,robot control,and multi-robot cooperation.)

王越超 中国科学院沈阳自动化研究所研究员.主要研究方向为机器人学.

E-mail:ycwang@sia.cn

(WANG Yue-ChaoProfessorat Shenyang Institute of Automation,Chinese Academy of Sciences.His research interest covers robotics.)

An Energy Suboptimal Recon fi guration Planning Approach to Wheel-manipulator Robots

HU Ya-Nan1,2MA Shu-Gen1,3LI Bin1WANG Ming-Hui1WANG Yue-Chao1

In recon fi guration planning of modular robots,coupling between goal assignment for individual modules and their trajectory planning leads to the combinatorial explosion problem.This paper proposes an energy suboptimal planning approach based on a simpli fi ed model.The problem of recon fi guration planning is transformed into an optimal control problem,which decouples goal assignment and trajectory planning.By solving the optimal control problem described by the Hamilton-Jacobi-Bellman(HJB)equation,a value function and optimal trajectories of the simpli fi ed model are derived.Respective goals of the modules are determined by attraction regions of the value function.Actual trajectories are obtained by searching in suboptimal regions that locate in the neighborhood of optimal trajectories of the simpli fi ed model.Simulation results show that the proposed approach can select a proper set of modules,and that generated trajectories can satisfy the dynamic constraint of the robot in an environment with obstacles.

Modular robots,recon fi guration planning,model reduction,optimal control

November 12,2015;accepted June 14,2016

胡亚南,马书根,李斌,王明辉,王越超.轮手一体机器人能量次优重构规划方法.自动化学报,2017,43(8):1358−1369

Hu Ya-Nan,Ma Shu-Gen,Li Bin,Wang Ming-Hui,Wang Yue-Chao.An energy suboptimal recon fi guration planning approach to wheel-manipulator robots.Acta Automatica Sinica,2017,43(8):1358−1369

2015-11-12 录用日期2016-06-14

国家自然科学基金(61473283)资助

Supported by National Natural Science Foundation of China(61473283)

本文责任编委侯增广

Recommended by Associate Editor HOU Zeng-Guang

1.中国科学院沈阳自动化研究所机器人学国家重点实验室 沈阳110016中国 2.中国科学院大学北京100049中国 3.日本立命馆大学理工学部机器人学系滋贺525-8577日本

1.State Key Laboratory of Robotics,Shenyang Institute of Automation,Chinese Academy of Sciences,Shenyang 110016,China 2.University of Chinese Academy of Sciences,Beijing 100049,China 3.Department of Robotics,Ritsumeikan University,Shiga-ken 525-8577,Japan

DOI10.16383/j.aas.2017.c150752