自动化学报  2017, Vol. 43 Issue (8): 1358-1369   PDF    
轮手一体机器人能量次优重构规划方法
胡亚南1,2, 马书根1,3, 李斌1, 王明辉1, 王越超1     
1. 中国科学院沈阳自动化研究所机器人学国家重点实验室 沈阳 110016 中国;
2. 中国科学院大学 北京 100049 中国;
3. 日本立命馆大学理工学部机器人学系 滋贺 525-8577 日本
摘要: 模块化机器人的重构规划中,由于各模块的目标分配与其轨迹规划之间的耦合关系导致组合爆炸问题.本文提出一种基于简化模型的能量次优规划方法,将重构规划问题转化为最优控制问题,实现目标分配与轨迹规划的解耦.通过求解由Hamilton-Jacobi-Bellman(HJB)方程描述的最优控制问题,得到简化模型的值函数和最优轨迹.各模块的运动目标由值函数的吸引域决定.通过在最优轨迹附近的次优区域内搜索得到实际运动轨迹,提高了搜索效率.仿真实验结果表明,该方法能够选择合适的模块组合,并能在障碍物环境中生成满足机器人动力学约束的运动轨迹.
关键词: 模块化机器人     重构规划     模型简化     最优控制    
An Energy Suboptimal Reconfiguration Planning Approach to Wheel-manipulator Robots
HU Ya-Nan1,2, MA Shu-Gen1,3, LI Bin1, WANG Ming-Hui1, WANG Yue-Chao1     
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
Manuscript received : November 12, 2015, accepted: June 14, 2016.
Foundation Item: Supported by National Natural Science Foundation of China (61473283)
Author brief: MA Shu-Gen    Professor in the Department of Robotics, 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:shugen@se.ritsumei.ac.jp;
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:libin@sia.cn;
WANG Ming-Hui    Professor at Shenyang Institute of Automation, Chinese Academy of Sciences. His research interest covers mobile robots, robot control, and multi-robot cooperation.E-mail:mhwang@sia.cn;
WANG Yue-Chao    Professor at Shenyang Institute of Automation, Chinese Academy of Sciences. His research interest covers robotics.E-mail:ycwang@sia.cn
Corresponding author. 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:robinvista2@gmail.com
Recommended by Associate Editor HOU Zeng-Guang
Abstract: In reconfiguration 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 simplified model. The problem of reconfiguration 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 simplified 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 simplified 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.
Key words: Modular robots     reconfiguration planning     model reduction     optimal control    

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

图 1 轮手一体机器人及其重构 Figure 1 Wheel-manipulator robots and their reconfiguration

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

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

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

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

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

图 2 二维无约束机器人重构例子 Figure 2 The reconfiguration example of unconstrained robots in two dimensions

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

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

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

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

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

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

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

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

2.1 基本数学知识

向量场表示为$(\textbf{R}^n, X)$, 其中X是光滑映射$X:\textbf{R}^n\rightarrow\textbf{R}^m$.如果光滑曲线$ {x}:I\rightarrow\textbf{R}^n$满足$ {x}(t)=X( {x}(t)), \forall t\in I$ (I$\textbf{R}$的开子集), 则称$ {x}:I\rightarrow\textbf{R}^n$是向量场${(\textbf{R}^n, X)}$的轨迹.为了强调轨迹的初始条件$ {x}(0) =x_{0}$, 通常将轨迹表示为$x(\cdot, x_{0})$.控制系统表示为$(\textbf{R}^n, \textbf{R}^m, F)$.其中F是光滑映射$F:\textbf{R}^n\times\textbf{R}^m\rightarrow\textbf{R}^n$.假设系统的状态空间是$\textbf{R}^n$, 对于它的一个分解$\textbf{R}^n=\textbf{R}^m\times\textbf{R}^k$, 可以定义投影$\pi_m:\textbf{R}^n\rightarrow \textbf{R}^{m}$$\pi_{k}:\textbf{R}^{n}\rightarrow \textbf{R}^k$.

定义1.向量场$(\textbf{R}^n, X)$$(\textbf{R}^m, Y)$被称为近似$\pi_m$相关[16], 如果存在$\mathcal{K}_{\infty}$类函数$\gamma$和常数$c\in\textbf{R}^{+}_0$使得对任意的初始状态$x\in\textbf{R}^{n}$和时间$t\in\textbf{R}^{+}_0$, 有$|\pi_m\circ x(t, x)- y(t, \pi_m(x))|<\gamma(|\pi_k(x)|)+c$.其中, $|x|$表示状态x的欧氏范数.系统完整模型对应的向量场与其简化模型对应的向量场近似相关意味着, 完整模型轨迹的投影与简化模型的轨迹是有界的, 其边界是初始状态的函数.

定义2.控制系统$(\textbf{R}^n, \textbf{R}^m, F)$被称为增量一致有界输入有界状态稳定(Incrementally uniformly bounded input bounded state stable, IUBIBSS), 如果存在$\mathcal{K}_{\infty}$类函数$\gamma_1$$\gamma_2$以及常数$d\in\textbf{R}^{+}_0$使得对所有$t\in\textbf{R}^+_0$, 初始状态$x_1, x_2\in\textbf{R}^n$和控制输入$\pmb{u}_1, \pmb{u}_2: \textbf{R}^+_0\rightarrow\textbf{R}^m$, 有$|x_{\pmb{u}_1}(t, x_1) -x_{\pmb{u}_2}(t, x_2) |\leq\gamma_1(|x_1-x_2|)+\gamma_2(\parallel \pmb{u}_1-\pmb{u}_2\parallel)+d$其中, $\parallel{f}\parallel=\mathrm{ess\sup}_{t\in[0, \tau]}|f(t)|$, 对于任意函数$f: [0, \tau]\rightarrow\textbf{R}^n, \tau\in\textbf{R}^+$.文献[16]给出了两个向量场近似相关的充分条件:

定理1.对于向量场$(\textbf{R}^n, X)$, 定义$F=T\pi_m\cdot X: \textbf{R}^m\times\textbf{R}^k\rightarrow\textbf{R}^m$是状态空间$\textbf{R}^m$上的控制系统.向量场$(\textbf{R}^m, Y)$定义为$Y(y)=T_{(y, 0) }\pi_m\cdot X(y, 0) =F(y, 0) .$如果$(\textbf{R}^n, X)$是沿纤维实际稳定的(Fiberwise practically stable), 且F满足IUBIBSS, 则向量场$(\textbf{R}^m, Y)$$(\textbf{R}^n, X)$是近似$\pi_m$相关的.

2.2 机器人运动模型分析

下面介绍轮手一体机器人的运动模型.单模块机器人的构型可由$\pmb Q=\{\pmb{g}_{Ib}, \pmb{q}\}\in SE(3) \!\times\!\textbf{R}^5$描述.其中, 矩阵$\pmb{g}_{Ib}$表示轮体坐标系$O_{b}x_{b}y_{b}z_{b}$相对于全局坐标系$O_{I}x_{I}y_{I}z_{I}$的位姿, 而向量$\pmb{q}$表示手臂关节转角.位姿矩阵$\pmb{g}_{Ib}$中的姿态存在多种表示方法, 本文选取欧拉角参数表示.因此, 单模块机器人的构型坐标为$\tilde {\pmb Q}=(x, y, z, \theta, \phi, \psi, q_1, q_2, \cdots, q_5) ^{\mathrm{T}}\in\textbf{R}^{11}$, 其中x, yz为轮体质心坐标, 而$\theta$, $\phi$$\psi$为轮体坐标系的$XYZ$欧拉角.轮手一体机器人的动力学模型[17]

$\begin{equation}\label{eq:1} {\pmb M\dot{\tilde{\pmb V}}+\pmb C\tilde{\pmb V}=\tilde{\pmb\tau}} \end{equation}$ (1)

其中, 机器人的广义速度$\tilde{\pmb V}=({\pmb V}_{\!Ib}^{\mathrm{T}}, {\dot{\pmb q}}^{\mathrm{T}})^{\mathrm{T}}\in\textbf{R}^{11}$包含轮体的刚体速度${\pmb V}_{\!Ib}=(v_x, v_y, v_z, \omega_x, \omega_y, \omega_z)^{\mathrm{T}}$和手臂关节速度${\dot{\pmb q}}=(\dot q_1, \dot q_2, \cdots, \dot q_5) ^{\mathrm{T}}$, 惯性矩阵$\pmb M$是手臂关节坐标$\pmb q$的函数, 而柯氏力和向心力矩阵$\pmb C$$\pmb q$$\tilde{\pmb V}$的函数($\pmb M$$\pmb C$的具体形式见附录).

机器人的构型$\tilde{\pmb Q}$与广义速度$\tilde{\pmb V}$存在一阶微分关系(具体形式见附录)

$\begin{equation}\label{eq:2} {\tilde{\pmb Q}=h(\tilde{\pmb V})} \end{equation}$ (2)

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

$\begin{equation}\label{eq:3} {\dot{\pmb x}=f(\pmb x, \pmb u)} \end{equation}$ (3)

其中, 状态向量$\pmb x=(\tilde{\pmb Q}^{\mathrm T}, \tilde{\pmb V}^{\mathrm T})^{\mathrm T}\in\textbf{R}^{22}$, $\pmb u$是控制输入.由于机器人的控制输入与状态存在线性关系, 式(3) 又可写成仿射形式

$\begin{equation}\label{eq:4} {\dot{\pmb x}=A(\pmb x)+B(\pmb x)\pmb u} \end{equation}$ (4)

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

$\begin{equation}\label{eq:5} {\int_{0}^{\infty}(\pmb x^{\mathrm T}\pmb x+\pmb u^{\mathrm T}\pmb u)\textrm{d}t} \end{equation}$ (5)

其中, 被积函数中的$\pmb x^{\mathrm T}\pmb x$项表示状态误差的代价(不失一般性, 假设目标状态位于原点), $\pmb u^{\mathrm T}\pmb u$项表示控制的代价.代价泛函的含义是用尽量小的控制量使机器人与目标状态保持较小的误差.

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

基于以上分析, 将机器人的状态分解为主要分量和次要分量, 即$\pmb x=(\pmb x_s^{\mathrm T}, \pmb x_r^{\mathrm T})^{\mathrm T}$.其中主要分量由变化较大的状态变量组成, 即$\pmb x_s=(x, y, \theta, v_x)^{\mathrm T}$.并由此定义投影$\pi_s:\pmb x\rightarrow\pmb x_s$$\pi_r:\pmb x\rightarrow\pmb x_r$.根据状态的分解, 式(4) 可以写成

$\begin{equation}\label{eq:6} {\dot{\pmb x}=A_s(\pmb x)+\underbrace{A(\pmb x)-A_s(\pmb x)}_{A_r(\pmb x)}+B(\pmb x)\pmb u} \end{equation}$ (6)

其中, $A_s(\pmb x)=A(\pmb x)|_{\pmb x_r=\pmb 0}$是主要状态分量$\pmb x_s$的函数. $A_r(\pmb x)$存在上界, 即$\parallel A_r(\pmb x)\parallel\leq A_{r\max}(\pmb x)$.原系统(4) 可视为系统$\dot{\pmb x}=A_s(\pmb x)+B(\pmb x)\pmb u$受到扰动$A_r(\pmb x)$后的结果.

由于未知参数和无法建模的因素, 模型与实际系统总存在误差.而且机器人在实际运动时可能受到外部扰动.因此, 在最优控制的基础上考虑一定的鲁棒控制更有实际意义.根据文献[18]的结论, 系统$\dot{\pmb x}=A_s(\pmb x)+B(\pmb x)\pmb u$在代价$\int_{0}^{\infty}(A_{r\max}^2(\pmb x)+\pmb x^{\mathrm T}\pmb x+\pmb u^{\mathrm T}\pmb u)\textrm{d}t$下的最优控制也是系统(4) 的鲁棒控制.通过求$\dot{\pmb x}=A_s(\pmb x)+B(\pmb x)\pmb u$的最优控制问题, 可以得到最优反馈控制律$\pmb u=k(\pmb x)$.

下面证明$T_{(\pmb x_s, 0) }\pi_s\cdot f(\pmb x_s, 0) $f近似$\pi_s$相关.

证明.定义代价泛函

$\begin{equation}\label{eq:7} V(\pmb x)=\min \limits_{\pmb u}\int_{0}^{\infty}(A_{r\max}^2(\pmb x)+\pmb x^{\mathrm T}\pmb x+\pmb u^{\mathrm T}\pmb u)\textrm{d}t \end{equation}$ (7)

在输入$\pmb u=k(\pmb x)$下, 系统(3) 为

$\begin{equation}\label{eq:8} \dot{\pmb x}=f(\pmb x, k(\pmb x)) \end{equation}$ (8)

$V(\pmb x)$可作为系统(8) 的Lyapunov函数, 且$\dot V(\pmb x)<0$[18].因此, 系统(8) 是渐近稳定的, 从而满足沿纤维实际稳定[16].

然后分析系统$F=T\pi_s\cdot f$的稳定性质.验证F的稳定性同样需要构造相应的Lyapunov函数.由于系统(8) 渐近稳定, 因此满足部分稳定(Partial stability).根据Lyapunov逆定理[19], 对于不同的主要状态$\pmb x_{s1}$, $\pmb x_{s2}$和次要状态$\pmb x_{r1}$, $\pmb x_{r2}$ (此时视为输入), 存在函数$V_1(\pmb x_{s1})$$V_2(\pmb x_{s2})$满足$V_1(\pmb x_{s1})>0$$V_2(\pmb x_{s2})>0$, 且有$\dot{V_1}(\pmb x_{s1})<0$$\dot{V_2}(\pmb x_{s2})<0$.定义$V(\pmb x_{s1}, \pmb x_{s1})=V_1(\pmb x_{s1})+V_2(\pmb x_{s2})$.所以$V(\pmb x_{s1}, \pmb x_{s1})$可以作为F的IUBIBSS Lyapunov函数, 因此F满足IUBIBSS.

根据定理1, $T_{(\pmb x_s, 0) }\pi_s\cdot f(\pmb x_s, 0) $是系统(8) 的近似相关向量场.

这说明系统(8) 可以近似简化为${\dot{\pmb x}}_s=T_{(\pmb x_s, 0) }\pi_s\cdot f(\pmb x_s, 0) :=f_s(\pmb x_s)$, 而${\dot{\pmb x}}_s=f_s(\pmb x_s)$就是系统(8) 的简化模型, 其最优轨迹与完整模型的轨迹有界, 且误差$\delta$是初始状态的函数, 如图 3所示.

图 3 完整模型与简化模型的运动轨迹 Figure 3 The trajectory of the full model and that of the simplified model

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

2.3 简化模型的形式

令式(8) 中的$\pmb x_r=0$可以得到简化模型$f_s$的形式

$\left\{ \begin{array}{l} \dot x = {v_x}\cos \theta \\ \dot y = {v_x}\sin \theta \\ \dot \theta = {u_\theta }\\ {{\dot v}_x} = {u_v} \end{array} \right.$ (9)

其中, $u_{\theta}$表示转向控制量, $u_v$是履带转动产生的加速度.为方便求解, 将式(9) 改写为以下形式

$\left\{ \begin{array}{l} \dot x = v\cos \theta \\ \dot y = v\sin \theta \\ \dot \theta = s|v|\\ \dot v = a + {a_e} \end{array} \right.$ (10)

其中, v表示轮体的前进速度$v_x$, a是轮体驱动器导致的加速度, $a_e$是外力造成的加速度, 例如重力, s表示机器人转向控制量.简化模型的状态为${\pmb x}_s$, 控制量为$\pmb u=(a, s)$.

3 简化模型的最优控制

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

3.1 最优控制问题描述

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

$\begin{equation}\label{eq:11} {L=c_{1}a^2+c_{2}v^2+c_{3}v+c_4} \end{equation}$ (11)

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

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

$\begin{equation}\label{eq:12} {\frac{mv^2}{r} \leq F_{\max}} \end{equation}$ (12)

从而得到另一约束关系$|s|\leq F_{\max}/(m v^2) $.因此, 定义可行控制空间为

$\begin{array}{l} U = \left\{ {(a,s)|{a_{\min }} \le a \le {a_{\max }},} \right.\\ \quad \quad \left. {|s| \le \min \left( {\frac{1}{{{r_m}}},\frac{{{F_{\max }}}}{{(m{v^2})}}} \right)} \right\} \end{array}$ (13)

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

$ - \mathop {\min }\limits_{\mathit{\boldsymbol{u}} \in {\rm{ }}\mathit{\boldsymbol{U}}} \{ \nabla V \cdot {f_s} + L\} = 0$ (14)

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

$\begin{array}{l} - \mathop {\min }\limits_{\mathit{\boldsymbol{u}} \in \mathit{\boldsymbol{U}}} \left\{ {{V_x}v\cos \theta + {V_y}v\sin \theta + {V_\theta }s|v| + } \right.\\ \quad \quad \quad \left. {{V_v}(a + {a_e}) + {c_1}{a^2} + {c_2}{v^2} + {c_3}v + {c_4}} \right\} = 0 \end{array}$ (15)
3.2 HJB方程数值求解

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

$\begin{array}{l} - {V_x}v\cos \theta - {V_y}v\sin \theta - \mathop {\min }\limits_s \{ {V_\theta }s|v|\} - \\ \mathop {\min }\limits_a \{ {V_v}(a + {a_e}) + {c_1}{a^2}\} - {c_2}{v^2} - {c_3}v - {c_4} = 0 \end{array}$ (16)

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

$\begin{equation}\label{eq:17} \begin{aligned} -\left(V_{x}v\cos\theta\right)_{i, j, k, w}= -|v\cos\theta|\frac{V_{i+\xi_{x}, j, k, w}-V_{i, j, k, w}}{h_x} \end{aligned} \end{equation}$ (17)

其中, $\xi_{x}=|v\cos\theta|$.

y方向的离散结果为

$\begin{equation}\label{eq:18} {-\left(V_{y}v\sin\theta\right)_{i, j, k, w}=-|v\sin\theta|\frac{V_{i, j+\xi_{y}, k, w}-V_{i, j, k, w}}{h_y}} \end{equation}$ (18)

其中, $\xi_{y}=|v\sin\theta|$.

$\theta$方向的离散结果为

$ - \mathop {\min }\limits_s \{ {V_\theta }s|v|\} = \left| {{V_\theta }} \right| \cdot \left| {sv} \right|$ (19)

根据Godunov方案, $|V_{\theta}|$的离散结果为

$\begin{array}{l} |{V_\theta }{|_{i,j,k,w}} = \max (\frac{{{V_{i,j,k,w}} - {V_{i,j,k + 1,w}}}}{{{h_\theta }}},\\ \quad \quad \quad \quad \quad \quad \quad \frac{{{V_{i,j,k,w}} - {V_{i,j,k - 1,w}}}}{{{h_\theta }}},0) \end{array}$ (20)

为使(19) 中的最小值符号成立, 最优控制s的符号应和$V_{\theta}$的符号相反, 幅值应为$\pmb U$中允许的最大幅值.

v方向记为

$\begin{equation}\label{eq:21} {-\min\limits_{a}\{c_{1}a^2+V_{v}a+V_{v}a_{e}\}} \end{equation}$ (21)

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

1) $-V_{v}/2c_{1}>a_{\max}$, 此时函数最小值取在$a=a_{\max}$处, 最小值为$\min\nolimits_{a}f_{a}(a)=V_{v}(a_{e}+ a_{\max})+c_{1}a_{\max}^{2}$.

2) $-V_{v}/2c_{1}<a_{\min}$, 此时函数最小值取在$a=a_{\min}$处, 最小值为$\min\nolimits_{a}f_{a}(a)=V_{v}(a_{e}+ a_{\min})+c_{1}a_{\min}^{2}$.

3) $a_{\min}<-V_{v}/2c_{1}<a_{\max}$, 此时函数最小值取在对称轴$a=-V_{v}/2c_{1}$处, 最小值为$\min\nolimits_{a}f_{a}(a)= -V_{v}^{2}/4c_{1}+a_{e}V_{v}$.

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

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

$\begin{equation}\label{eq:22} \begin{aligned} &-\left(V_{v}(a_e+a_{\max})\right)_{i, j, k, w}=\\ &\qquad-|a_{e}+a_{\max}|\frac{V_{i, j, k, w+{\zeta}_1}-V_{i, j, k, w}}{h_v} \end{aligned} \end{equation}$ (22)

其中, ${\zeta}_1=\mathrm{sgn}(a_{e}+a_{\max})$, $\mathrm{sgn}$是符号函数.

2) 与情况1类似, 有

$\begin{equation}\label{eq:23} \begin{aligned} &-\left(V_{v}(a_e+a_{\min})\right)_{i, j, k, w}=\\ &\qquad-|a_{e}+a_{\min}|\frac{V_{i, j, k, w+{\zeta}_2}-V_{i, j, k, w}}{h_v} \end{aligned} \end{equation}$ (23)

其中, ${\zeta}_2=\mathrm{sgn}(a_{e}+a_{\min})$.

3) 此时将$f_{a}$视为$V_v$的函数, 其最值取在

$\begin{equation}\label{eq:24} {V_{v}=-\frac{-a_{e}}{2\left(\dfrac{1}{4c_1}\right)}=2c_{1}a_{e}} \end{equation}$ (24)

此时有$\min\nolimits_{a}f_{a}(a)=c_{1}a_{e}^{2}$.

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

$\begin{equation}\label{eq:25} {V_{i, j, k, w}\!=\!\hat{f}(V_{i\pm 1, j, k, w}, V_{i, j\pm 1, k, w}, V_{i, j, k\pm 1, w}, V_{i, j, k, w\pm 1})} \end{equation}$ (25)

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

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

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

$\begin{equation}\label{eq:26} {\pmb t=(\cos\theta, \sin\theta, M_{x}\cos\theta+M_{y}\sin\theta)\rho} \end{equation}$ (26)

其中, $\rho=1/\sqrt{1+(M_{x}\cos\theta+M_{y}\sin\theta)^2}$, $M_{x}={\partial M}/{\partial x}$.

单位法向量为

$\begin{equation}\label{eq:27} \pmb n=(-M_x, -M_y, 1) \eta \end{equation}$ (27)

其中, $\eta=1/\sqrt{1+M_{x}^2+M_{y}^2}$.

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

$\begin{equation}\label{eq:28} {\kappa_g=\dot{\pmb t}\cdot(\pmb n\times\pmb t)} \end{equation}$ (28)

其中, "$\times$"表示差积.轨迹的曲率应满足:

$\begin{equation}\label{eq:29} {\kappa_g\geq\frac{|v|}{r_m}} \end{equation}$ (29)

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

$\begin{equation}\label{eq:30} \begin{aligned} &s\leq\frac{\eta}{({\rho}^2r_m)}-(M_{y}\mathrm{c}\theta-M_{x}\mathrm{s}\theta)\big((M_{xx}\mathrm{c}\theta+\\ &\qquad M_{xy}\mathrm{s}\theta)\dot x+\frac{(M_{xy}\mathrm{c}\theta+M_{yy}\mathrm{s}\theta)\dot y\big){\eta}^2}{|v|} \end{aligned} \end{equation}$ (30)

其中, $M_{xy}={\partial^2 M}/({\partial x \partial y})$.

机器人沿全局坐标系$xy$轴的速度$\dot x$$\dot y$是三维空间的速度向$x\!-\!y$平面的投影, 即

$\begin{equation}\label{eq:31} \left\{ \begin{aligned}&\dot x=v\cos\alpha\cos\theta=v\rho\cos\theta\\&\dot y=v\cos\alpha\sin\theta=v\rho\sin\theta \end{aligned} \right. \end{equation}$ (31)

其中, $\alpha$$\pmb t$$\pmb t$$x\!-\!y$平面的投影$\pmb t'$间的夹角, 如图 4所示.如果不考虑其他阻力, 则外部加速度$a_e$全部由重力产生, 其幅值$|a_e|=\pmb g\cdot \pmb t$, 其中$\pmb g$是重力加速度.而$a_e$的符号可以根据向量$\pmb n$$\pmb t'$的夹角判断, 夹角大于$90^{\circ}$说明机器人在上坡, 应取负; 反之, 说明在下坡, 应取正.

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

图 5 不同初始位姿及单目标下简化模型的最优轨迹 Figure 5 The optimal trajectories of the simplified model with a single goal and different initial poses
4 轨迹规划

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

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

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

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

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

首先建立法向力的模型, t时刻$x_{i}y_{j}$坐标处的法向应力为

$\begin{equation}\label{eq:32} \sigma(x_{i}y_{j}, t)=\left(\frac{k_c}{w_t}+k_{\phi}\right)(\Delta z_{ij})^{n_t}-C\Delta{\dot{z}_{ij}} \end{equation}$ (32)

其中, $k_c$, $k_{\phi}$是反应地面特征的常数, $w_t$是履带的宽度, $\Delta z_{ij}$$x_{i}y_{j}$坐标处履带的沉陷深度, $n_t$为沉陷指数, C为阻尼系数.

假设$ij$单元内的应力分布均匀, 同为$\sigma(x_{i}y_{j}, t)$. $x_{i}y_{j}$坐标处的法向力代表其所在单元内的法向合力, 即应力与单元面积的乘积

$\begin{equation}\label{eq:33} F_{n, ij}=\sigma(x_{i}y_{j}, t)\Delta A_{ij} \end{equation}$ (33)

履带的切向应变$j_x(x_{i}y_{j}, t)$也是位置和时间的函数, 其满足如下微分方程

$\begin{equation}\label{eq:34} \frac{\textrm{d}j_x(x_{i}y_{j}, t)}{\textrm{d}t}+\frac{r_w|\omega_w|}{x_i}j_x(x_{i}y_{j}, t)=r_w\omega_w-v_x \end{equation}$ (34)

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

解出$j_x(x_{i}y_{j}, t)$后可计算$x_{i}y_{j}$处的切向应力

$\begin{equation}\label{eq:35} \tau _x(x_{i}y_{j}, t) = (c + \sigma(x_{i}y_{j}, t)\tan \varphi )\left(1 - {\textrm{e}^{ - \frac{|{j_x({x_{i}y_{j}}, t)}|}{K}}}\right) \end{equation}$ (35)

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

根据切向应力可计算$x_{i}y_{j}$坐标处的切向力

$\begin{equation}\label{eq:36} {F_{t, }}_{ij} = {\mathop{\rm sgn}} (j_x({x_{i}y_{j}}, t)){\tau _x}(x_{i}y_{j}, t) \Delta {A_{ij}} \end{equation}$ (36)

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

$\begin{equation}\label{eq:37} {F_{l, }}_{ij} = {\mathop{\rm sgn}} (j_y({x_{i}y_{j}}, t)){\tau _y}(x_{i}y_{j}, t) \Delta {A_{ij}} \end{equation}$ (37)

为将上述力分量引入机器人的动力学模型, 需将其表示为力旋量$\pmb F_{ij} = \left(F_{t, ij}, F_{l, ij}, F_{n, ij}, 0, 0, 0\right)^{\rm{T}}$, 并借助雅克比矩阵将其转化为广义力.

$\begin{equation}\label{eq:38} \tilde{\pmb\tau} = \sum\limits_{i, j}{\pmb{J}_{ij}^{\rm{T}}{\pmb{F}_{ij}}} \end{equation}$ (38)

其中, $\pmb{J}_{ij}=\rm{Ad}_{\pmb{g}_{\mathit{c_{ij}b}}}\left[\pmb I_{6\times 6}\;\;\pmb 0_{6\times 11}\right]$, 而$\pmb{g}_{\mathit{c_{ij}b}}$是位于$x_{i}y_{j}$处的单元坐标系$O_{c_{ij}}x_{c_{ij}}y_{c_{ij}}z_{c_{ij}}$到轮体坐标系$O_{b}x_{b}y_{b}z_{b}$的变换矩阵.对于手臂上的导轮, 本文采用文献[26]中的硬质车轮接触模型对其受力进行描述.

4.2 轨迹搜索方法

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

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

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

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

$\begin{equation}\label{eq:40} \left\{ \begin{aligned} &\omega_w=\mathrm{PI}(a_r)\\ &\pmb q=\rm{InverseKinematics(\mathit{s_r})}\\ \end{aligned} \right. \end{equation}$ (39)

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

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

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

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

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

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

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

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

5 仿真验证

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

表 1 仿真参数 Table 1 The simulation parameters

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

图 7 三角构形重构例子 Figure 7 The reconfiguration example of the triangle configuration

实例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)所示.

图 8 串形构形重构例子 Figure 8 The reconfiguration example of the serial configuration

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

图 9 二维无约束机器人距离最优重构的值函数 Figure 9 The value function of the reconfiguration of two dimensional unconstrained robots with optimal distance
6 结论

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

附录

轮手一体机器人的动力学方程${\pmb M\dot{\tilde{\pmb V}}+\pmb C\tilde{\pmb V}=\tilde{\pmb\tau}}$中, 惯性矩阵$\pmb M$由轮体的惯性矩阵, 手臂连杆的惯性矩阵以及二者的耦合项组成[17], 其具体形式为

$\begin{array}{l} \mathit{\boldsymbol{M}}(\mathit{\boldsymbol{q}}) = \left[ {\begin{array}{*{20}{c}} {{\mathit{\boldsymbol{M}}_b}} & {{{\bf{0}}_{6 \times 1}}}\\ {{{\bf{0}}_{5 \times 6}}} & {{{\bf{0}}_{5 \times 1}}} \end{array}} \right] + \\ \quad \sum\limits_{i = 1}^5 {\left[ {\begin{array}{*{20}{c}} {{\rm{Ad}}_{{\mathit{\boldsymbol{g}}_{{L_i}b}}}^{\rm{T}}{\mathit{\boldsymbol{M}}_i}{\rm{A}}{{\rm{d}}_{{\mathit{\boldsymbol{g}}_{{L_i}b}}}}} & {{\rm{Ad}}_{{\mathit{\boldsymbol{g}}_{{L_i}b}}}^{\rm{T}}{\mathit{\boldsymbol{M}}_i}{\rm{A}}{{\rm{d}}_{{\mathit{\boldsymbol{g}}_{{L_i}b}}}}{\mathit{\boldsymbol{J}}_{is}}}\\ {\mathit{\boldsymbol{J}}_{is}^{\rm{T}}{\rm{Ad}}_{{\mathit{\boldsymbol{g}}_{{L_i}b}}}^{\rm{T}}{\mathit{\boldsymbol{M}}_i}{\rm{A}}{{\rm{d}}_{{\mathit{\boldsymbol{g}}_{{L_i}b}}}}} & {\mathit{\boldsymbol{J}}_{is}^{\rm{T}}{\rm{Ad}}_{{\mathit{\boldsymbol{g}}_{{L_i}b}}}^{\rm{T}}{\mathit{\boldsymbol{M}}_i}{\rm{A}}{{\rm{d}}_{{\mathit{\boldsymbol{g}}_{{L_i}b}}}}{\mathit{\boldsymbol{J}}_{is}}} \end{array}} \right]} A1 \end{array}$ (A1)

其中, $\pmb M_{b}=\left[ \begin{array}{cc} m_{b}{\pmb I}_{3\!\times\!3} & \pmb 0_{3 \!\times \!3} \\ \pmb 0_{3 \!\times \!3}& {\mathcal I}_{b} \\ \end{array}\right]$为轮体的广义惯性矩阵, $\pmb M_{i}=\left[ \begin{array}{cc} m_{i}{\pmb I}_{3\!\times\!3} & \pmb 0_{3 \!\times \!3} \\ \pmb 0_{3 \!\times \!3}& {\mathcal I}_{i} \\ \end{array}\right]$为手臂第i个连杆的广义惯性矩阵. ${\pmb J}_{is}$为手臂第i个连杆质心坐标系$O_{L_i}x_{L_i}y_{L_i}z_{L_i}$的空间雅克比矩阵, 其是手臂关节向量$\pmb q$的函数.

柯氏力和向心力矩阵$\pmb C$的第$ij$项的形式为

$\begin{array}{l} {\mathit{\boldsymbol{C}}_{ij}}(\mathit{\boldsymbol{\widetilde V}}) = \sum\limits_{k = 1}^{11} {\left( {\frac{{\partial {\mathit{\boldsymbol{M}}_{ij}}}}{{\partial {\mathit{\boldsymbol{\phi}} _k}}} - \frac{1}{2}\frac{{\partial {\mathit{\boldsymbol{M}}_{kj}}}}{{\partial {\mathit{\boldsymbol{\phi}} _k}}}} \right)} {|_{\mathit{\boldsymbol{\phi}} = 0}}{\mathit{\boldsymbol{\widetilde V}}_k} + \\ \quad \sum\limits_{k = 1}^{11} {\sum\limits_{l = 1}^{11} {\left( {\frac{{\partial {\mathit{\boldsymbol{S}}_{li}}}}{{\partial {\mathit{\boldsymbol{\phi}} _k}}} - \frac{{\partial {\mathit{\boldsymbol{S}}_{lk}}}}{{\partial {\mathit{\boldsymbol{\phi}} _i}}}} \right)} } {|_{\mathit{\boldsymbol{\phi}} = 0}}{\mathit{\boldsymbol{\widetilde V}}_k}{\mathit{\boldsymbol{M}}_{lj}} \end{array}$ (A2)

其中, $\pmb\phi$为李代数坐标, 广义速度$\tilde{\pmb V}$与李代数坐标的关系为$\tilde{\pmb V}={\pmb S}(\pmb\phi)\dot{\pmb\phi}$.

轮手一体机器人的运动学方程${\tilde{\pmb Q}=h(\tilde{\pmb V})}$的具体形式为:

$\left\{ {\begin{array}{*{20}{l}} {\dot x = ({\rm{c}}\phi {\rm{c}}\psi {\rm{c}}\theta - {\rm{s}}\theta {\rm{s}}\psi ){v_x} - ({\rm{c}}\psi {\rm{s}}\theta + {\rm{c}}\phi {\rm{s}}\psi {\rm{c}}\theta ){v_y} + {\rm{c}}\theta {\rm{s}}\phi {v_z}}\\ {\dot y = ({\rm{c}}\phi {\rm{c}}\psi {\rm{s}}\theta + {\rm{c}}\theta {\rm{s}}\psi ){v_x} + ({\rm{c}}\psi {\rm{c}}\theta - {\rm{c}}\phi {\rm{s}}\psi {\rm{s}}\theta ){v_y} + {\rm{s}}\theta {\rm{s}}\phi {v_z}}\\ {\dot z = {\rm{c}}\phi {v_z} + ({\rm{s}}\psi {v_y} - {\rm{c}}\psi {v_x}){\rm{s}}\phi \dot \theta = ({\rm{s}}\psi {\omega _y} - {\rm{c}}\psi {\omega _x})/{\rm{s}}\phi }\\ {\dot \phi = {\rm{s}}\psi {\omega _x} + {\rm{c}}\psi {\omega _y}}\\ {\dot \psi = ({\rm{c}}\psi {\omega _x} - {\rm{s}}\psi {\omega _y})/\tan \phi + {\omega _z}}\\ {{{\dot q}_i} = {{\dot q}_i},i = 1 \sim 5} \end{array}} \right.$ (A3)

其中, $\mathrm{c}\theta=\cos\theta$, $\mathrm{s}\theta=\sin\theta$, 余下同理.

参考文献
1
Yim M, Shen W M, Salemi B, Rus D, Moll M, Lipson H, Klavins E, Chirikjian G S. Modular self-reconfigurable 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 reconfigurable planetary rover. Chinese Journal of Mechanical Engineering, 2005, 41(12): 190-195.
( 贺鑫元, 马书根, 李斌, 王越超. 可重构星球探测机器人的机构设计. 机械工程学报, 2005, 41(12): 190-195. DOI:10.3321/j.issn:0577-6686.2005.12.038)
3
Bhat P, Kuffner J, Goldstein S, Srinivasa S. Hierarchical motion planning for self-reconfigurable 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, Rieffel E, Suh J, Yim M. On the general reconfiguration 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, Kokaji S. A motion planning method for a self-reconfigurable 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. Reconfiguration 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, Kuffner J J Jr. Randomized kinodynamic planning. The International Journal of Robotics Research, 2001, 20(5): 378-400. DOI:10.1177/02783640122067453
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. DOI:10.1109/70.760342
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. DOI:10.1177/0278364906075328
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. DOI:10.1177/02783649922066385
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 simplified 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. DOI:10.1109/70.744607
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 configuration 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. DOI:10.1080/00207179208934374
19
Khalil H K. Nonlinear Systems. 3rd Edition. Beijing: Publishing House of Electronics Industry, 2012, 116-120.
( KhalilH K. 非线性系统. 第3版. 北京: 电子工业出版社, 2012, 116-120.)
20
Bardi M, Capuzzo-Dolcetta I. Optimal Control and Viscosity Solutions of Hamilton-Jacobi-Bellman Equations. Boston:Birkhäuser, 1997.
21
Crandall M G, Lions P L. Two approximations of solutions of Hamilton-Jacobi equations. Mathematics of Computation, 1984, 43: 1-19. DOI:10.1090/S0025-5718-1984-0744921-8
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. DOI:10.1137/S0036142901396533
24
Takei R, Tsai R. Optimal trajectories of curvature constrained motion in the Hamilton-Jacobi formulation. Journal of Scientific Computing, 2013, 54(2-3): 622-644. DOI:10.1007/s10915-012-9671-y
25
Solis J M, Longoria R G. Modeling track-terrain interaction for transient robotic vehicle maneuvers. Journal of Terramechanics, 2008, 45(3): 65-78. DOI:10.1016/j.jterra.2008.07.003
26
Huang M H, Orin D E. Dynamic simulation of actively-coordinated 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