空间两刚体滚动约束系统在线运动规划

任书锋，杨 丹，余海东，王 皓,b

(上海交通大学 a.上海市复杂薄板结构数字化制造重点实验室；b.机械系统与振动国家重点实验室，上海 200240)

机械手灵巧操作和球形机器人的规划控制是机器人研究领域的热点，涉及到与物体的接触与滑动接触，纯滚动接触能量损耗小，构成一类非完整系统[1]，可使用较少的驱动器实现灵巧操作，有利于降低成本并提高系统可靠性.然而，系统状态变量之间难以解耦带来了控制和规划上的困难，同时，滚动约束系统不能转为链式结构，不具备微分平坦的良好特性，且该系统不满足幂零条件[2]，无法使用相关的控制算法.因此，建立一种可以用于实际机器人系统的滚动约束运动规划方法十分重要.

国内外学者对于非完整系统的研究主要可分为基于最优化的方法和增量随机采样的方法.前者通过构造目标函数和非线性约束，将运动规划问题转换成非线性优化问题，代表方法有打靶法、直接配点法、迭代线性二次调节器(iLQR)和最优动作控制(SAC)[3-6].其中，直接配点法通过设置配点将运动规划问题转换为非线性规划问题，对初始状态量和控制量的取值不敏感，算法较为稳定.iLQR将非线性系统线性化，然后使用梯度下降的方法，迭代计算得到最优轨迹.SAC算法可以实现快速在线运动规划.增量随机采样方法的代表为快速搜索随机树(RRT)[7]及其改进算法.例如：RRT*，Bi-RRT和Goal-bias RRT等，基于采样的方法具有效率低、规划路径不连续的缺点，同时对于非完整约束，基于控制状态空间采样的方法可能导致不收敛，而基于运动状态空间采样的方法在扩展子节点时仍需解决两点边值问题[8].

滚动约束系统运动规划方法按照接触模型可分为针对球-平面间的滚动接触模型的控制方法[9-12]，如幂零近似迭代驱动[9]、基于高斯-博内定理的分阶段控制方法[10]和基于同伦延拓法[11]的数值优化方法.针对两个球之间滚动接触模型的控制方法，文献[13]利用球的运动封闭性质提出了球在一般三维物体上滚动的运动规划算法，文献[14]提出了球与具有恒定曲率物体滚动接触的控制方法，文献[15]将球-平面模型运动规划方法拓展到高维球体在高维欧式空间中.上述研究成果针对某一特定模型建立，并且都没有考虑实际机器人系统中的在线控制，这些方法通过离线一次性完成运动规划，如果应用到实际机械系统的实时控制，由于计算效率的问题，重新计算的控制输入可能已经不适用于当前状态.

针对已有研究成果的局限性，本文建立了滚动约束系统的实时运动规划算法，分别选取常见的球-平面模型和球-球模型作为仿真对象，通过传感器的反馈更新系统状态，使用滚动优化的方式满足对实时规划的要求，同时改进了SAC算法中滚动预测的迭代方法，以适用于系统变量高度耦合的滚动约束系统.另外，结合球-平面模型和球-球模型的仿真模拟，验证了算法对这两类常见滚动约束机器人系统的有效性.

1 滚动约束运动学模型

1.1 接触模型坐标系系统

两个刚体点接触模型如图1所示，图中：Oxsyszs为整体坐标系；Ox1y1z1、Ox2y2z2分别为固连在两个刚体上的正交坐标系；Oxc1yc1zc1、Oxc2yc2zc2分别为接触点在物体1、2上的正交坐标系；nc1、nc2分别为物体1、2上接触点坐标系的法向量；ψ为接触点处坐标系Oxc1yc1zc1和Oxc2yc2zc2之间的相对转角，将坐标系Oxc2yc2zc2绕着坐标系Oxc1yc1zc1的n轴转动ψ角度后，两坐标系的x轴重合.在Oxc2yc2zc2中定义2个物体间的相对线速度V=[vxvyvz]，相对角速度Ω=[ωxωyωz].

图1 接触模型坐标系Fig.1 Coordinate system of contact model

建立三维物体曲面W⊂R3的参数表达式，存在二维平面上的开子集U⊂R2到该曲面W的一个可逆坐标映射f:U⊂R2→W⊂R3，使用(u,v)表示该二维平面参数坐标，定义(f,U)为曲面W的参数化表达，对于球面的正交坐标系可为

f:U→R3:(u,v)

(ρsinucosv,ρsinusinv,ρcosu)

(1)

式中：ρ为球半径；0

基于正交坐标系，定义曲面上某一点f(U)处的单位高斯坐标系为

(2)

1.2 滚动约束运动方程

两个三维刚体滚动接触运动模型如图2所示，考虑空间物体1受到与物体2的距离为ρ1+ρ2(ρ1为物体1Ox1y1z1到Ox1y1z1的距离；ρ2物体2Ox2y2z2到Oxc2yc2zc2的距离)的约束，因此物体1相对于物体2有5个自由度，该位形空间可用q=[u1v1u2v2ψ]5个参数表示，并使用U1=[u1v1]表示接触点在物体1曲面上的位置，U2=[u2v2]表示接触点在物体2曲面上的位置.

图2 空间刚体滚动接触运动模型Fig.2 Model of rolling contact motion for space rigid bodies

根据文献[16]的结果，2个三维物体纯滚动的一阶运动学方程为

(3)

同时，保持两物体接触的约束条件为

vz=0

(4)

两物体间纯滚运动的约束条件为

vx=0,vy=0,ωz=0

(5)

式(3)可表示为如下的控制仿射形式：

(6)

式中：t为控制时间；q(t)为系统状态变量；c(t)=[-ωyωx]T为系统控制输入.

(7)

2 滚动约束运动规划算法

2.1 问题的描述

非完整系统的运动规划问题可以转换为最优控制问题：寻找满足非完整约束式(6)的一个最优c*(t)(t∈[0,T]，T为控制结束时间)，并满足起终点状态约束：

q(0)=qstart，q(T)=qgoal

(8)

式中：qstart为起点的位形；qgoal为终点的位形.

同时使如下二次型性能指标函数为极小：

(9)

l(q(t),c(t))=0.5[q(t)-qd(t)]T×

Q(q(t)-qd(t))+0.5c(t)TRc(t)dt

(10)

m(q(T))=0.5[q(T)-qd(T)]T×

P1[q(T)-qd(T)]

(11)

式中：m[q(T)]为对终点状态偏差的权重；Q为动态轨迹跟踪偏差的半正定权重矩阵；qd(t)为通过离线规划获得的参考轨迹；R为系统控制能量输入的正定权重矩阵；P1为计算与目标点偏差的半正定权重矩阵.

除起终点约束外，对于三维空间流形，需要解决奇异点的问题，一种可行的方法是用多个坐标映射构建一个图册.为简化问题，通过引入不等式约束避免奇异点，例如对式(1)中球面参数坐标系在极点位置u=0和u=π存在2个奇异点，可设置如下约束：

0

(12)

同时，对两物体的相对角速度幅值c建立如下约束：

cmin≤c≤cmax

(13)

式中：cmin为最小控制输入向量；cmax为最大控制输入向量.

为简化表达，使用ξ(t)代表(q(t),c(t))，上述最优控制问题可表示为如下形式.

输入：物体曲面模型、qstart及qgoal.

输出：满足式(8)、(12)及(13)约束且使性能指标函数式(9)最小的最优c*(t)(t∈[0,T])，即

(14)

式中：h(ξ(t))包括式(6)非完整约束和式(8)起终止状态约束方程；g(ξ(t))包括式(12)的奇异点约束和式(13)的控制变量约束方程.

2.2 运动规划算法

针对上述建立的最优控制问题，建立如图3所示的在线控制算法框图，实现系统的在线控制，图中c1为名义控制输入.

图3 在线控制算法框图Fig.3 Online motion planning algorithm

第一阶段为离线运动规划，使用配点法快速获得名义轨迹.将运动规划过程分为N-1段，每段两端节点称为配点，子区间表示为[ti,ti+1],i=0,1,2,…,N-1，hi=ti+1-ti表示区间时长，使用qi代表ti时刻的系统状态.对每个区间，采用多项式样条描述轨迹，使用辛普森积分求解运动微分约束方程和成本函数的积分来平衡对计算效率和精确度的要求.具体实现方法如下：

(15)

式中：c0～c3为多项式系数.

同时在区间端点处满足：

(16)

式中：fi为式(7)中非线性方程在第i点计算得到的当前接触点位形速度.

求解式(15)和(16)构成的线性方程组，可以获得c0～c3的表达式，并将t=(ti+ti+1)/2代入式(15)，获得区间中点的状态变量的表达式为

(17)

使用辛普森积分计算性能指标函数：

(18)

在每个子区间上使用辛普森积分计算非完整约束：

(19)

式(18)中li+1/2和(19)中fi+1/2均为式(17)中点状态qi+1/2的函数.

因此，式(14)表达的系统离线运动规划问题可以表示为如下的非线性规划问题：

s.t.h(q(ti)∶q(tN);c(t0)∶c(tN-1))=0

g(q(ti)∶q(tN);c(t0)∶c(tN-1))≤0

(20)

针对上述非线性规划问题，使用与目标轨迹的距离作为性能指标，为获得轨迹最短的优化结果，使用初始点和目标点间的线性插值作为配点法的目标轨迹.

在使用配点法优化轨迹时，迭代初值的选取对最终的求解结果有很大影响，在实际应用中，常对5个状态变量轨迹有不同要求，例如球形机器人在平面滚动，期望平面上的接触点轨迹最短，而对球面接触点轨迹和物体间相对转角轨迹要求满足起终点约束即可，应选择使得平面上接触点轨迹为直线的控制输入作为配点法的初始值.图3中两状态控制法的目的即是通过控制5个系统状态中两个状态轨迹为起终点间线性插值，使用式(3)反向求解获得对应的控制输入ctc(t).具体实现方法如下：

(21)

式中：[1]、[2]分别表示提取q的第1、第2个元素.

由式(3)的第1个方程可得：

c(ti)=[K1(ti)+Rψ(ti)K2(ti)Rψ(ti)]

(22)

由式(3)的其余两个方程，使用欧拉法得到其余3个状态变量轨迹的计算公式为

RψK2(ti)Rψ]-1c(ti)

(23)

从i=0,1,2,…,M-1交替使用式(22)和(23)得到c(ti)和q(ti)，获得两状态控制下的轨迹qtc(t)和控制输入ctc(t).

使用两状态控制法的结果ξtc=[qtcctc]作为初始值，求解式(20)的非线性规划问题，可以获得维度为5(N+1)优化轨迹qdc矩阵和维度为2(N+1)的控制输入cdc.

首先将非线性模型式(6)线性化：

(24)

式中：

在预测区间内，最优控制律可以表达为

(25)

对应的系统状态变量求解形式为

q(t)=

(26)

式中：f1(s)=f[q(s)c1(s)],f2(s)=f[q(s)c2(s)].

构建预测区间内运动规划的二次型函数J1：

(27)

(28)

(29)

(30)

式中：η(t)为梯度方向；ε为步长.由此得

(31)

(32)

(33)

式中：Jτ为关于时间的单变量函数.使Jτ最小的控制介入时间即为最优控制介入时间τ.

步骤3通过线搜索迭代求解最优控制时长λ：

λ=ωkΔtinit

(34)

式中：ω为搜索速率；k为迭代次数；Δtinit为控制时长的初值.将获得的最优控制律作用在[t0,t0+ts]，在获得反馈更新后，根据反馈结果更新状态，重新计算预测窗口内的最优控制律并应用到下一个采样周期内，直到控制任务结束.

3 仿真计算及结果分析

3.1 仿真参数

基于上述建立的滚动约束系统在线规划算法，通过MATLAB软件实现球-平面模型和球-球模型的仿真，仿真程序运行环境为：i7-4770MQ CPU@3.40 GHz 12 GB RAM.规划周期均设置为1 s，使用序列二次规划(SQP)算法实现非线性规划问题式(20)的求解，获得参考轨迹和名义控制输入，在实时运动规划过程中应用四阶龙格库塔法求解非完整约束式(7).

离线规划中配点法使用表1中的参数，配置41个配点，设置相应的控制变量幅值约束，对Q,P1和R的选取主要考虑轨迹与起终点直线轨迹qd的动态跟踪误差最小，因此使用较大的Q和较小的R，P1用于增加目标点的权重，可根据对不同系统变量的要求在对角线位置选择不同的数值.

使用表2中的参数作为在线运动规划的仿真参

表1 滚动接触模型离线运动规划参数Tab.1 Parameters for offline planning of rolling contact model

表2 滚动接触模型在线运动规划参数Tab.2 Parameters for online planning of rolling contact model

数，模拟反馈控制频率为50 Hz，设置预测时间窗口为0.5 s，求解式(7)预测模型运动，并将模拟结果作为传感器的反馈输入.表中kmax为迭代次数的最大值.

3.2 滚动约束系统的可控性

在真实系统中，应建立具有可控性的滚动约束模型，对于滚动约束非完整系统的可控性，已有学者进行了相关研究.文献[17]给出任意两个空间三维物体滚动约束系统可控性的一般性结论：当且仅当两个物体的曲面空间是非等距同构时，任意两个空间三维物体滚动约束系统是完全可控的.

应用上述结论，在任意模型参数下，球模型和平面模型是非等距同构的，因此球-平面滚动接触模型是完全可控的.对于球-球接触模型，当且仅当两球的半径不同时，两个球为非等距同构，因此需要建立不同球半径的接触模型，即ρ1≠ρ2.

3.3 球-平面接触模型运动规划仿真

对于球在平面滚动的运动规划仿真，标记平面模型为物体1，标记球模型为物体2，使用式(1)作为球参数表达式，选择球半径ρ=0.5.设置起点位形为qstart=[π/4 0 π/2 0 0]，目标点位形为qgoal=[π/2 π/2 π/2 0 0].

图4所示为该仿真案例中，滚动接触模型在t=0，0.25，0.75，1 s时刻的状态，并标注两个物体上接触点的最终优化轨迹.对比起终点位置处球模型上固连坐标系的状态，证明球面接触点的位置[u2v2]以及ψ保持一致，图中灰色平面表示接触平面，紫色球体表示运动过程中的接触球体.

图4 球-平面滚动接触运动规划结果Fig.4 Planning results of sphere-plane rolling contact motion

图5所示为球-平面模型仿真结果，模型的5个位形坐标在t=0.9 s附近提前达到预定的目标位置，图例中qg(i)(i=1,2,…,5)表示目标点的5个位形坐标，其中球面接触点轨迹构成一条闭合曲线以满足起终点位置相同的要求，并且轨迹上的箭头标明了球面接触点闭合轨迹的运动方向.由于离线规划采用低阶的方法求解非完整约束方程，所以获得的参考轨迹在真实机器人系统中并不能满足非完整约束，而在实时控制中采用高阶方法求解约束获得的轨迹与真实系统更加接近，因此离线与在线规划结果存在一定差异，同时在线规划中通过调整预测窗口长度的方式可以快速得到下一采样区间的优化控制输入，并在获得新的反馈后修正结果，不断滚动迭代.

图5 球-平面模型优化控制输入和状态空间曲线Fig.5 Optimized control input and state space curves of sphere-plane model

图6 球-平面模型计算时间曲线Fig.6 tc versus t of sphere-plane model

图6所示为球-平面模型的每步的计算时间tc与仿真时间(即前文控制时间)的关系，在该案例中，计算时间为6.5～8.9 ms，单步平均求解时长为7.1 ms.在仿真中，采样时间为20 ms，考虑到使用MATLAB软件计算效率较其他编译型语言低，建立的控制器可用于中低速运动的场景.

3.4 球-球接触模型运动规划仿真

对于球在另一个球面滚动的运动规划仿真，标记半径较大的球模型为物体1，另外一个球模型为物体2，设置两个球模型的半径参数分别为ρ1=4，ρ2=1，同样使用式(1)作为球参数表达式.设置起点位形为qstart=[π/4 0 π/2 0 0]，目标点位形为qgoal=[π/2 π/4 π/2 0 0].

图7所示为该仿真案例中，滚动接触模型在t=0，0.25，0.75,1 s时刻的状态，并显示两个物体上接触点的最终优化轨迹，终点处物体2上接触点位置与起点处相同，并且达到物体1上的目标位置.

图8所示为球-球模型仿真结果，5个位形坐标在t=1 s时刻达到预定的目标位置，由于对物体1上接触点参考轨迹设置的权重较大，在线规划的最终轨迹与物体1上的参考轨迹有较高的重合度.

图7 球-球滚动接触仿运动规划结果Fig.7 Planning results of sphere-sphere rolling contact simulation motion

实际上，如果SAC算法直接使用起终点的线性插值作为目标轨迹，会出现无法到达目标点的情况，因此离线规划的目的是为在线规划提供一条较好的参考轨迹，使得在线规划每一次滚动优化的目标值都具有一定可行性，并且离线规划可以根据实际情况调整配点数量和积分方法平衡参考轨迹的精度和计算时间.

图9所示为球-球模型的每步的计算时间与仿真时间的关系，在该案例中，计算时间为6.7～13.0 ms，单步平均求解时长为8.1 ms，计算效率较球-平面模型低.

图8 球-球模型优化控制输入和状态空间曲线Fig.8 Optimized control input and state space curves of sphere-sphere model

图9 球-球模型计算时间曲线Fig.9 Calculation time curve of sphere-sphere model

4 结语

本文基于纯滚动接触运动学模型，构建了含滚动约束这一类非完整系统的在线运动规划算法框架.针对已有的研究成果受限于特定的模型且提出的算法只适用于离线的、全局的运动规划的问题，改进了SAC算法，采用离线运动规划结合在线运动规划的方式，建立了通用性更强的在线运动规划算法.通过配点法快速获得低精确度的参考轨迹，在实时规划过程中通过反馈校正和滚动优化获得下一步的优化控制输入.将算法用于球-平面模型和球-球模型的仿真，结果证明该算法可用于解决球形机器人地面运动控制和机械手指操作物体的实时运动规划问题，也为其他滚动接触机器人系统的控制算法提供了可行的解决方案.