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

doi:10.16183/j.cnki.jsjtu.2019.261

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

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

上海交通大学 a.上海市复杂薄板结构数字化制造重点实验室, 上海 200240

b.机械系统与振动国家重点实验室, 上海 200240

Online Motion Planning for Two Space Rigid Bodies with Rolling Constraints

REN Shufeng^a, YANG Dan^a, YU Haidong^,^a, Wang Hao^a^,^b

a. Shanghai Key Laboratory of Digital Manufacture for Thin-Walled Structures, Shanghai 200240, China

b. State Key Laboratory of Mechanical System and Vibration, Shanghai Jiao Tong University, Shanghai 200240, China

通讯作者: 余海东,男,副教授,电话(Tel.):021-34206542;E-mail:hdyu@sjtu.edu.cn.

责任编辑: 陈晓燕

收稿日期: 2019-09-15

基金资助:

国家科技重大专项(2017ZX04005001)

Received: 2019-09-15

作者简介 About authors

任书锋(1995-),男,山西省孝义市人,硕士生,从事机器人运动规划的研究

摘要

空间两刚体间的滚动约束系统是一种典型的非完整系统,非完整的特性可以用于简化机械结构,提高系统可靠性.针对纯滚动约束非完整系统的状态变量之间相互耦合难以控制、已有的控制方法局限于特定的模型且缺少对在线控制研究等问题,建立了适用于一般滚动约束系统在线运动规划的求解方法.该方法基于滚动约束一阶运动模型,首先通过配点法实现离线运动规划获得参考轨迹,然后在实时控制中结合滚动优化框架运用最优动作控制(SAC)算法,实现滚动系统的在线运动规划.将算法运用于球-平面滚动模型和两个球体间滚动模型的实时运动规划,仿真结果表明该方法在拓宽球形机器人控制和灵巧机械手操作方面具有实际应用价值.

关键词： 滚动约束; 非完整系统; 运动规划; 直接配点法; 最优动作控制

Abstract

The rolling restraint system between two space rigid bodies is a typical non-holonomic system. The incomplete characteristics can be used to simplify the mechanical structure and improve the reliability of the system. Aimed at the problems that the state variables of the pure rolling constraint non-holonomic system are difficult to control, the existing control methods are limited to specific models, and there is a lack of online control research, a solution method suitable for the online motion planning of the general rolling constraint system is established based on the rolling constraint first-order motion model. First, the offline motion planning is achieved by using the collocation method to obtain the reference trajectory. Then, the sequential action control (SAC) algorithm is used in real-time control combined with the rolling optimization framework to realize the online motion planning of the rolling system. The algorithm is applied to the real-time motion planning of the ball-plane rolling model and the rolling model between two spheres. The simulation results show that the method has a practical application value in broadening the control of the spherical robot and the operation of the dexterous manipulator.

Keywords： rolling constraints; non-holonomic system; motion planning; direct collocation method; sequential action control (SAC)

PDF (2429KB) 元数据多维度评价相关文章导出 EndNote| Ris| Bibtex 收藏本文

本文引用格式

任书锋, 杨丹, 余海东, 王皓. 空间两刚体滚动约束系统在线运动规划[J]. 上海交通大学学报（自然版）, 2021, 55(8): 1009-1017 doi:10.16183/j.cnki.jsjtu.2019.261

REN Shufeng, YANG Dan, YU Haidong, Wang Hao. Online Motion Planning for Two Space Rigid Bodies with Rolling Constraints[J]. Journal of shanghai Jiaotong University, 2021, 55(8): 1009-1017 doi:10.16183/j.cnki.jsjtu.2019.261

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

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

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

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

1 滚动约束运动学模型

1.1 接触模型坐标系系统

两个刚体点接触模型如图1所示,图中: $O x_{s} y_{s} z_{s} 为整体坐标系; O x_{1} y_{1} z_{1} 、 O x_{2} y_{2} z_{2} 分别为固连在两个刚体上的正交坐标系; O x_{c 1} y_{c 1} z_{c 1} 、 O x_{c 2} y_{c 2} z_{c 2} 分别为接触点在物体 1 、 2 上的正交坐标系; n_{c 1} 、 n_{c 2} 分别为物体 1 、 2 上接触点坐标系的法向量; ψ 为接触点处坐标系 O x_{c 1} y_{c 1} z_{c 1} 和 O x_{c 2} y_{c 2} z_{c 2} 之间的相对转角, 将坐标系 O x_{c 2} y_{c 2} z_{c 2} 绕着坐标系 O x_{c 1} y_{c 1} z_{c 1} 的 n 轴转动 ψ 角度后, 两坐标系的 x 轴重合 . 在 O x_{c 2} y_{c 2} z_{c 2} 中定义 2 个物体间的相对线速度 V = [v_{x} v_{y} v_{z}], 相对角速度 Ω = [ω_{x} ω_{y} ω_{z}] .$

图1

新窗口打开| 下载原图ZIP| 生成PPT

图1 接触模型坐标系

Fig.1 Coordinate system of contact model

建立三维物体曲面 $W \subset R^{3} 的参数表达式, 存在二维平面上的开子集 U \subset R^{2} 到该曲面 W 的一个可逆坐标映射 f : U \subset R^{2} \to W \subset R^{3}, 使用 (u, v) 表示该二维平面参数坐标, 定义 (f, U) 为曲面 W$ 的参数化表达,对于球面的正交坐标系可为

(1)

\begin{array}{l} f : U \to R^{3} : (u, v) \\ (ρ \sin u \cos v, ρ \sin u \sin v, ρ \cos u) \end{array}

式中:ρ为球半径;0<u<π,-π<v<π.

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

(2)

\begin{matrix} \begin{array}{l} x (U) = \frac{\partial_{u} (U)}{‖ \partial_{u} (U) ‖} \\ y (U) = \frac{\partial_{v} (U)}{‖ \partial_{v} (U) ‖} \\ n (U) = \frac{x \times y}{‖ x \times y ‖} \end{array}\} \end{matrix}

1.2 滚动约束运动方程

两个三维刚体滚动接触运动模型如图2所示,考虑空间物体1受到与物体2的距离为 $ρ_{1} + ρ_{2} (ρ_{1} 为物体 1 O x_{1} y_{1} z_{1} 到 O x_{1} y_{1} z_{1} 的距离; ρ_{2} 物体 2 O x_{2} y_{2} z_{2} 到 O x_{c 2} y_{c 2} z_{c 2} 的距离) 的约束, 因此物体 1 相对于物体 2 有 5 个自由度, 该位形空间可用 q = [u_{1} v_{1} u_{2} v_{2} ψ] 5 个参数表示, 并使用 U_{1} = [u_{1} v_{1}] 表示接触点在物体 1 曲面上的位置, U_{2} = [u_{2} v_{2}]$ 表示接触点在物体2曲面上的位置.

图2

新窗口打开| 下载原图ZIP| 生成PPT

图2 空间刚体滚动接触运动模型

Fig.2 Model of rolling contact motion for space rigid bodies

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

(3)

\begin{array}{l} {\overset{\cdot}{U}}_{1} = (M_{1})^{- 1} (K_{1} + R_{ψ} K_{2} R_{ψ})^{- 1} [(\begin{array}{l} - ω_{y} \\ ω_{x} \end{array}) - {\dot{K}}_{2} (\begin{array}{l} v_{x} \\ v_{y} \end{array})] \\ {\overset{\cdot}{U}}_{2} = (M_{2})^{- 1} R_{ψ} (K_{1} + R_{ψ} K_{2} R_{ψ})^{- 1} [(\begin{array}{l} - ω_{y} \\ ω_{x} \end{array}) + K_{1} (\begin{array}{l} v_{x} \\ v_{y} \end{array})] \\ \overset{\cdot}{ψ} = T_{1} M_{1} {\overset{\cdot}{U}}_{1} + T_{2} M_{2} {\overset{\cdot}{U}}_{2} - ω_{z} \end{array}

式中: ${\overset{\cdot}{U}}_{1} 、 {\overset{\cdot}{U}}_{2} 及 \overset{\cdot}{ψ} 为接触点位形速度; M_{i} 、 K_{i} 及 T_{i} (i = 1,2) 分别为物体 i 在接触点处的度量张量、曲率张量及挠率张量; R_{ψ} 为坐标系 O x_{c 1} y_{c 1} z_{c 1} 和 O x_{c 2} y_{c 2} z_{c 2} 关于 xOy$ 平面坐标系的旋转矩阵.

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

(4)

\begin{matrix} v_{z} = 0 \end{matrix}

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

(5)

\begin{matrix} v_{x} = 0, v_{y} = 0, ω_{z} = 0 \end{matrix}

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

(6)

\begin{matrix} \overset{\cdot}{q} = f (q (t), c (t)) \end{matrix}

式中: $t 为控制时间; q (t) 为系统状态变量; c (t) = [- ω_{y} ω_{x}]^{T}$ 为系统控制输入.

(7)

f = [\begin{array}{l} M_{1}^{- 1} \\ M_{2}^{- 1} \\ T_{1} + T_{2} R_{ψ} \end{array}] (K_{1} + R_{ψ} K_{2} R_{ψ})^{- 1} [\begin{array}{l} - ω_{y} \\ ω_{x} \end{array}]

2 滚动约束运动规划算法

2.1 问题的描述

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

(8)

\begin{matrix} q (0) = q_{start}, q (T) = q_{goal} \end{matrix}

式中:q_start为起点的位形;q_goal为终点的位形.

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

(9)

J (q (t), c (t)) = \int_{0}^{T} l (q (t), c (t)) d t + m (q (T))

(10)

l (q (t), c (t)) = 0.5 [q (t) - q_{d} {(t)]}^{T} \times Q (q (t) - q_{d} (t)) + 0.5 c {(t)}^{T} Rc (t) d t

(11)

m (q (T)) = 0.5 [q (T) - q_{d} {(T)]}^{T} \times P_{1} [q (T) - q_{d} (T)]

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

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

(12)

\begin{matrix} 0 < u (t) < π \end{matrix}

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

(13)

\begin{matrix} c_{\min} \leq c \leq c_{\max} \end{matrix}

式中:c_min为最小控制输入向量;c_max为最大控制输入向量.

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

输入:物体曲面模型、q_start及q_goal.

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

(14)

\begin{matrix} \begin{array}{l} \min_{c (t)} J (ξ (t)) \\ s.t. h (ξ (t)) = 0, g (ξ (t)) \leq 0 \end{array}\} \end{matrix}

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

2.2 运动规划算法

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

图3

新窗口打开| 下载原图ZIP| 生成PPT

图3 在线控制算法框图

Fig.3 Online motion planning algorithm

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

首先利用插值函数计算区间中点处的状态变量 $q_{i + 1 / 2} . 使用 \hat{q} (t) 表示轨迹插值函数, 区间 [t_{i}, t_{i + 1}]$ 上轨迹的三次埃尔米特插值函数可表示为

(15)

\begin{matrix} \hat{q} (t) = c_{0} + c_{1} t + c_{2} t^{2} + c_{3} t^{3} \end{matrix}

式中:c₀~c₃为多项式系数.

同时在区间端点处满足:

(16)

\begin{matrix} \begin{array}{l} \hat{q} (t_{i}) = q_{i}, & d \hat{q} (t_{i}) / d t = f_{i} \\ \hat{q} (t_{i + 1}) = q_{i + 1}, & d \hat{q} (t_{i + 1}) / d t = f_{i + 1} \end{array}\} \end{matrix}

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

求解式(15)和(16)构成的线性方程组,可以获得 $c_{0} ~ c_{3} 的表达式, 并将 t = (t_{i} + t_{i + 1}) / 2$ 代入式(15),获得区间中点的状态变量的表达式为

(17)

\begin{matrix} q_{i + \frac{1}{2}} = \frac{1}{2} (q_{i} + q_{i + 1}) + \frac{h_{i}}{8} (f_{i} - f_{i + 1}) \end{matrix}

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

(18)

\begin{array}{l} J = \overset{N}{\sum_{i = 0}} l (q (t_{i}), c (t_{i})) h_{i} + m (q (T)) = \\ \overset{N - 1}{\sum_{i = 0}} \frac{h_{i}}{6} (l_{i} + 4 l_{i + \frac{1}{2}} + l_{i + 1}) h_{i} + m (q (t_{N})) \end{array}

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

(19)

q (t_{i + 1}) - q (t_{i}) = \frac{1}{6} h_{i} (f_{i} + 4 f_{i + \frac{1}{2}} + f_{i + 1})

式(18)中 $l_{i + 1 / 2} 和 (19) 中 f_{i + 1 / 2} 均为式 (17) 中点状态 q_{i + 1 / 2}$ 的函数.

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

(20)

\begin{array}{l} \min_{ξ} \overset{N}{\sum_{i = 0}} l (q (t_{i}), c (t_{i})) h_{i} + m (q (T)) \\ s.t. h (q (t_{i}) ∶ q (t_{N}); c (t_{0}) ∶ c (t_{N - 1})) = 0 \\ g (q (t_{i}) ∶ q (t_{N}); c (t_{0}) ∶ c (t_{N - 1})) \leq 0 \end{array}

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

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

将运动规划整个过程分为 $M 段, 每个子区间为 [t_{i}, t_{i + 1}], i = 0, 1, \dots, M - 1, h_{i} = t_{i + 1} - t_{i} 表示区间时长 . 选取物体 1 上的接触点轨迹 [u_{1} (t) v_{1} (t)] 为直线轨迹, 该直线初始轨迹 {u^{d}}_{1} (t) 和 {v^{d}}_{1} (t)$ 可表示为

(21)

\begin{matrix} \begin{array}{l} {u^{d}}_{1} (t_{i}) = \frac{q (T) [1] - q (0) [1]}{T} t_{i} \\ {v^{d}}_{1} (t_{i}) = \frac{q (T) [2] - q (0) [2]}{T} t_{i} \end{array}\} \end{matrix}

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

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

(22)

\begin{matrix} c (t_{i}) = [K_{1} (t_{i}) + R_{ψ} (t_{i}) K_{2} (t_{i}) R_{ψ} (t_{i})] M_{1} (t_{i}) {\overset{\cdot}{U}}_{1} (t_{i}) \end{matrix}

式中: ${\overset{\cdot}{U}}_{1} (t_{i}) = {[{u^{d}}_{1} (t_{i + 1}) {v^{d}}_{1} (t_{i + 1})] - [{u^{d}}_{1} (t_{i}) {v^{d}}_{1} (t_{i})]} / h_{i} .$

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

(23)

\begin{array}{l} {\overset{\cdot}{U}}_{2} (t_{i}) = [M_{2} (t_{i} {)]}^{- 1} R_{ψ} [K_{1} (t_{i}) + \\ R_{ψ} K_{2} (t_{i}) R_{ψ}]^{- 1} c (t_{i}) \\ \overset{\cdot}{ψ} (t_{i}) = T_{1} (t_{i}) M_{1} (t_{i}) {\overset{\cdot}{U}}_{1} (t_{i}) + T_{2} (t_{i}) M_{2} (t_{i}) {\overset{\cdot}{U}}_{2} (t_{i}) \\ U_{2} (t_{i + 1}) = U_{2} (t_{i}) + {\overset{\cdot}{U}}_{2} (t_{i}) h_{i} \\ ψ (t_{i + 1}) = ψ (t_{i}) + \overset{\cdot}{ψ} (t_{i}) h_{i} \end{array}

$\begin{array}{l} 从i = 0, 1, 2, \dots, M - 1 交替使用式 (22) 和 (23) 得到 c (t_{i}) 和 q (t_{i}), 获得两状态控制下的轨迹 q_{tc} (t) 和控制输入 c_{tc} (t) . \\ 使用两状态控制法的结果 ξ_{tc} = [q_{tc} c_{tc}] 作为初始值, 求解式 (20) 的非线性规划问题, 可以获得维度为 5 (N + 1) 优化轨迹 q_{dc} 矩阵和维度为 2 (N + 1) 的控制输入 c_{dc} . \end{array}$

第二阶段为在线运动规划,使用离线规划的结果 $ξ_{dc} = [q_{dc} c_{dc}] 分别作为在线规划的名义轨迹 q_{d} (t) 和名义控制输入 c_{1} (t), 进行实时运动规划 . 假设系统传感器采样时间为 t_{s}, 在 t_{0} 时刻 (t_{0} \in [0, t_{s}, 2 t_{s}, \dots, T]), 计算 [t_{0}, t_{0} + T_{p}] (T_{p} 为预测时长, 满足 t_{s} < T_{p} \leq T) 预测区间内的最优控制律, 但仅使用 [t_{0}, t_{0} + t_{s}] 时间段的控制律, 直到下一个时刻 t_{0} + t_{s}, 使用传感器反馈结果更新当前状态, 不断滚动优化直到到达目标点 . 每一次优化过程中计算的最优控制律包括 : 最优控制 c_{2}^{*} 、最优控制介入时间 τ 和最优控制时长 λ . 设 t_{f} = \min {t_{0} + T_{p}, T}, 给出 [t_{0}, t_{f}]$ 预测区间内最优控制律的求解方法.

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

(24)

\begin{matrix} \overset{\cdot}{q} = A (t) q + B (t) c \end{matrix}

式中:

$\begin{matrix} A (t) = \frac{\partial f (q, c)}{\partial q}, B (t) = \frac{\partial f (q, c)}{\partial c} \end{matrix}$

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

(25)

\begin{matrix} c (t) = \{\begin{array}{l} c_{1} (t), & t < τ, τ + λ < t < t_{f} \\ c_{2}^{*} (t), & τ \leq t \leq τ + λ \end{array} \end{matrix}

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

(26)

\begin{matrix} q (t) = \{\begin{array}{l} q (t_{0}) + \int_{t_{0}}^{t} f_{1} (s) d s, t_{0} \leq t < τ \\ q (t_{0}) + \int_{t_{0}}^{τ} f_{1} (s) d s + \int_{τ}^{t} f_{2} (s) d s, \\ τ \leq t < τ + λ \\ q (t_{0}) + \int_{t_{0}}^{τ} f_{1} (s) d s + \int_{τ}^{τ} f_{2} (s) d s + \\ \int_{τ}^{+} f_{1} (s) d s, τ + λ \leq t \leq t_{f} \end{array} \end{matrix}

式中: $f_{1} (s) = f [q (s) c_{1} (s)], f_{2} (s) = f [q (s) c_{2} (s)] .$

构建预测区间内运动规划的二次型函数J₁:

(27)

\begin{matrix} J_{1} (q (t), c (t)) = \int_{t_{0}}^{t_{f}} l (q (t), c (t)) d t \end{matrix}

步骤1 求解最优控制 $c_{2}^{*}$ (t).为计算目标函数的最大梯度下降方向,以 $c_{2}^{*}$ (τ)为控制变量,求解目标函数J₁关于最优控制时长λ→0⁺ (0⁺表示从正方向趋近于0)的导数:

(28)

\begin{array}{l} \frac{d J_{1}}{d λ^{+}} (τ, c_{2}^{*} (τ)) = ρ^{T} [f_{2} (q (τ)) - f_{1} (q (τ))] \\ \overset{\cdot}{ρ} = - A^{T} ρ - \frac{\partial {l^{T}}_{1}}{\partial q (t)} \end{array}\}

式中: $\frac{d J_{1}}{d λ^{+}}$ (τ, $c_{2}^{*}$ (τ))(λ⁺为最优控制时长)表示在τ时刻控制输入按照 $c_{2}^{*}$ 作用极短时间对目标函数J₁的影响.为获得使目标函数J₁梯度下降最快的最优控制律,并避免陷入局部最优,构建关于 $\frac{d J_{1}}{d λ^{+}}$ (τ, $c_{2}^{*}$ (τ))的二次型函数式J₂:

(29)

\begin{array}{l} J_{2} = \int_{t_{0}}^{t_{f}} l_{2} (t, c_{2}^{*} (t)) d t \\ l_{2} = \frac{1}{2} {\frac{d J_{1}}{d λ^{+}} (t, c_{2}^{*} (t)) - γ J_{1}}^{2} + \\ \frac{1}{2} c_{2}^{* T} (t) R c_{2}^{*} (t) \end{array}\}

式中: $γ J_{1} (γ \in [- 15, - 1]) 为梯度下降上限, γ 为系数 . J_{2} 为控制输入 c_{2}^{*} 的函数, 与状态变量无关, 因此 J_{2}$ 在任意梯度方向导数的零点即为该二次型函数极小值点,即

(30)

\begin{array}{l} D J_{2} η (t) = \frac{d}{d ε} \int_{t_{0}}^{t_{f}} l_{2} (t, c_{2}^{*} (t) + εη (t)) d t |_{ε = 0} = \\ \int_{t_{0}}^{t_{f}} \frac{\partial l_{2} (t, c_{2}^{*} (t))}{\partial c_{2}^{*} (t)} η (t) d t = 0, \forall η \end{array}

式中: $η (t) 为梯度方向; ε$ 为步长.由此得

(31)

\begin{array}{l} \frac{\partial l_{2}}{\partial c_{2}^{*}} = [ρ^{T} B (t) (c_{2}^{*} - c_{1}) - γ J_{1}] ρ^{T} B (t) + \\ c_{2}^{* T} R = 0 \end{array}

推导出最优控制 $c_{2}^{*}$ 的表达式:

(32)

c_{2}^{*} (t) = (B^{T} ρ ρ^{T} B + R^{T})^{- 1} (c_{1} + B^{T} ργ J_{1})

步骤2 求解最优控制介入时间 $τ, 使得在该时间点引入最优控制可以使 \frac{d J_{1}}{d λ^{+}} (τ, c_{2}^{*} (τ)) 最小, 添加控制输入能量和等待时间的正则项 (t - t_{0})^{2},$ 构建最优控制时间的目标函数:

(33)

\begin{matrix} J_{τ} = \frac{d J_{1}}{d λ^{+}} + ‖ c_{2}^{*} (t) ‖ + (t - t_{0})^{2} \end{matrix}

式中: $J_{τ} 为关于时间的单变量函数 . 使 J_{τ} 最小的控制介入时间即为最优控制介入时间 τ .$

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

(34)

\begin{matrix} λ = ω^{k} Δ t_{init} \end{matrix}

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

3 仿真计算及结果分析

3.1 仿真参数

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

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

表1 滚动接触模型离线运动规划参数

Tab.1 Parameters for offline planning of rolling contact model

参数名	参数值
配点数	41
控制变量约束	$\|ω_{x}\|$ ≤30, $\|ω_{y}\|$ ≤30
Q	diag([100 100 10 10 1])
R	diag([0.1 0.1])
P₁	diag([20 20 10 10 1])

新窗口打开| 下载CSV

使用表2中的参数作为在线运动规划的仿真参数,模拟反馈控制频率为50 Hz,设置预测时间窗口为0.5 s,求解式(7)预测模型运动,并将模拟结果作为传感器的反馈输入.表中k_max为迭代次数的最大值.

表2 滚动接触模型在线运动规划参数

Tab.2 Parameters for online planning of rolling contact model

参数名	参数值
传感器采样频率	f_s=50 Hz,t_s=0.02 s
预测区间/控制时长初值	T_p=0.5 s,Δt_init=0.1
目标函数下降上限系数	γ=-5
线搜索参数	ω=0.55,k_max=6
Q	diag([1000 1000 50 50 1])
R	diag([0.1 0.1])

新窗口打开| 下载CSV

3.2 滚动约束系统的可控性

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

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

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

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

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

图4

新窗口打开| 下载原图ZIP| 生成PPT

图4 球-平面滚动接触运动规划结果

Fig.4 Planning results of sphere-plane rolling contact motion

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

图5

新窗口打开| 下载原图ZIP| 生成PPT

图5 球-平面模型优化控制输入和状态空间曲线

Fig.5 Optimized control input and state space curves of sphere-plane model

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

图6

新窗口打开| 下载原图ZIP| 生成PPT

图6 球-平面模型计算时间曲线

Fig.6 t_c versus t of sphere-plane model

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

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

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

图7

新窗口打开| 下载原图ZIP| 生成PPT

图7 球-球滚动接触仿运动规划结果

Fig.7 Planning results of sphere-sphere rolling contact simulation motion

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

图8

新窗口打开| 下载原图ZIP| 生成PPT

图8 球-球模型优化控制输入和状态空间曲线

Fig.8 Optimized control input and state space curves of sphere-sphere model

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

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

图9

新窗口打开| 下载原图ZIP| 生成PPT

图9 球-球模型计算时间曲线

Fig.9 Calculation time curve of sphere-sphere model

4 结语

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

参考文献

原文顺序

文献年度倒序

文中引用次数倒序

被引期刊影响因子

[1]

徐娜, 陈雄, 孔庆生, 等.

非完整约束下的机器人运动规划算法

[J]. 机器人, 2011, 33(6):666-672.

[本文引用: 1]

, CHEN

Xiong

, KONG

Qingsheng

, et al.

Motion planning for robot with nonholonomic constraints

[J]. Robot, 2011, 33(6):666-672.

[本文引用: 1]

[2]

MURRAY

R M

, LI

Z X

, SASTRY

S S

. A mathematical introduction to robotic manipulation[M]. California: CRC Press, 2017.

[本文引用: 1]

[3]

彭坤, 彭睿, 黄震, 等.

求解最优月球软着陆轨道的隐式打靶法

[J]. 航空学报, 2019, 40(7):159-167.

[本文引用: 1]

PENG

Kun

, PENG

Rui

, HUANG

Zhen

, et al.

Implicit shooting method to solve optimal Lunar soft landing trajectory

[J]. Acta Aeronautica et Astronautica Sinica, 2019, 40(7):159-167.

[本文引用: 1]

[4]

周誌元, 谭天乐.

小行星探测器轨迹优化方法

[J]. 上海航天, 2014, 31(2):57-64.

[本文引用: 1]

ZHOU

Zhiyuan

, TAN

Tianle

Optimization for asteroids spacecraft trajectory

[J]. Aerospace Shanghai, 2014, 31(2):57-64.

[本文引用: 1]

[5]

YANG

C G

, LI

Z J

, LI

Trajectory planning and optimized adaptive control for a class of wheeled inverted pendulum vehicle models

[J]. IEEE Transactions on Cybernetics, 2013, 43(1):24-36.

DOI:10.1109/TSMCB.2012.2198813 URL [本文引用: 1]

[6]

ANSARI

A R

, MURPHEY

T D

Sequential action control: Closed-form optimal control for nonlinear and nonsmooth systems

[J]. IEEE Transactions on Robotics, 2016, 32(5):1196-1214.

DOI:10.1109/TRO.2016.2596768 URL [本文引用: 1]

[7]

杜明博, 梅涛, 陈佳佳, 等.

复杂环境下基于RRT的智能车辆运动规划算法

[J]. 机器人, 2015, 37(4):443-450.

[本文引用: 1]

Mingbo

, MEI

Tao

, CHEN

Jiajia

, et al.

RRT-based motion planning algorithm for intelligent vehicle in complex environments

[J]. Robot, 2015, 37(4):443-450.

[本文引用: 1]

[8]

PALMIERI

, ARRAS

K O

A novel RRT extend function for efficient and smooth mobile robot motion planning

[C]//2014 IEEE/RSJ International Conference on Intelligent Robots and Systems. Piscataway, NJ, USA: IEEE, 2014: 205-211.

[本文引用: 1]

[9]

ORIOLO

, VENDITTELLI

A framework for the stabilization of general nonholonomic systems with an application to the plate-ball mechanism

[J]. IEEE Transactions on Robotics, 2005, 21(2):162-175.

DOI:10.1109/TRO.2004.839231 URL [本文引用: 2]

[10]

, CANNY

Motion of two rigid bodies with rolling constraint

[J]. IEEE Transactions on Robotics and Automation, 1990, 6(1):62-72.

DOI:10.1109/70.88118 URL [本文引用: 2]

[11]

ALOUGES

, CHITOUR

, LONG

R X

A motion-planning algorithm for the rolling-body problem

[J]. IEEE Transactions on Robotics, 2010, 26(5):827-836.

DOI:10.1109/TRO.2010.2053733 URL [本文引用: 2]

[12]

赵振, 刘才山, 鲁建东.

空间物体点接触纯滚动的几何意义

[J]. 北京大学学报(自然科学版), 2016, 52(4):713-716.

[本文引用: 1]

ZHAO

Zhen

, LIU

Caishan

, LU

Jiandong

On nonholonomic constraints about the pure rolling of point contact

[J]. Acta Scientiarum Naturalium Universitatis Pekinensis, 2016, 52(4):713-716.

[本文引用: 1]

[13]

REHAN

, REYHANOGLU

Global formulation and motion planning for a sphere rolling on a smooth surface

[J]. International Journal of Control, Automation and Systems, 2018, 16(6):2709-2717.

DOI:10.1007/s12555-018-0011-3 URL [本文引用: 1]

[14]

JURDJEVIC

, ZIMMERMAN

Rolling sphere problems on spaces of constant curvature

[J]. Mathematical Proceedings of the Cambridge Philosophical Society, 2008, 144(3):729-747.

DOI:10.1017/S0305004108001084 URL [本文引用: 1]

[15]

ZIMMERMAN

J A

Optimal control of the sphere S ⁿ rolling on Eⁿ

[J]. Mathematics of Control, Signals and Systems, 2005, 17(1):14-37.

DOI:10.1007/s00498-004-0143-2 URL [本文引用: 1]

[16]

MONTANA

D J

The kinematics of contact and grasp

[J]. The International Journal of Robotics Research, 1988, 7(3):17-32.

DOI:10.1177/027836498800700302 URL [本文引用: 1]

[17]

AGRACHEV

A A

, SACHKOV

Y L

Control theory from the geometric viewpoint

[M]. Berlin, Heidelberg: Springer Berlin Heidelberg, 2004.

[本文引用: 1]

非完整约束下的机器人运动规划算法

2011

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

非完整约束下的机器人运动规划算法

2011

2017

求解最优月球软着陆轨道的隐式打靶法

2019

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

求解最优月球软着陆轨道的隐式打靶法

2019

小行星探测器轨迹优化方法

2014

小行星探测器轨迹优化方法

2014

Trajectory planning and optimized adaptive control for a class of wheeled inverted pendulum vehicle models

2013

Sequential action control: Closed-form optimal control for nonlinear and nonsmooth systems

2016

复杂环境下基于RRT的智能车辆运动规划算法

2015

复杂环境下基于RRT的智能车辆运动规划算法

2015

A novel RRT extend function for efficient and smooth mobile robot motion planning

2014

A framework for the stabilization of general nonholonomic systems with an application to the plate-ball mechanism

2005

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

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

Motion of two rigid bodies with rolling constraint

1990

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

A motion-planning algorithm for the rolling-body problem

2010

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

空间物体点接触纯滚动的几何意义

2016

空间物体点接触纯滚动的几何意义

2016

Global formulation and motion planning for a sphere rolling on a smooth surface

2018

Rolling sphere problems on spaces of constant curvature

2008

Optimal control of the sphere S ⁿ rolling on Eⁿ

2005

The kinematics of contact and grasp

1988

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

Control theory from the geometric viewpoint

2004

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

〈

〉