空间两刚体滚动约束系统在线运动规划
Online Motion Planning for Two Space Rigid Bodies with Rolling Constraints
通讯作者: 余海东,男,副教授,电话(Tel.):021-34206542;E-mail:hdyu@sjtu.edu.cn.
责任编辑: 陈晓燕
收稿日期: 2019-09-15
基金资助: |
|
Received: 2019-09-15
作者简介 About authors
任书锋(1995-),男,山西省孝义市人,硕士生,从事机器人运动规划的研究
空间两刚体间的滚动约束系统是一种典型的非完整系统,非完整的特性可以用于简化机械结构,提高系统可靠性.针对纯滚动约束非完整系统的状态变量之间相互耦合难以控制、已有的控制方法局限于特定的模型且缺少对在线控制研究等问题,建立了适用于一般滚动约束系统在线运动规划的求解方法.该方法基于滚动约束一阶运动模型,首先通过配点法实现离线运动规划获得参考轨迹,然后在实时控制中结合滚动优化框架运用最优动作控制(SAC)算法,实现滚动系统的在线运动规划.将算法运用于球-平面滚动模型和两个球体间滚动模型的实时运动规划,仿真结果表明该方法在拓宽球形机器人控制和灵巧机械手操作方面具有实际应用价值.
关键词:
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:
本文引用格式
任书锋, 杨丹, 余海东, 王皓.
REN Shufeng, YANG Dan, YU Haidong, Wang Hao.
国内外学者对于非完整系统的研究主要可分为基于最优化的方法和增量随机采样的方法.前者通过构造目标函数和非线性约束,将运动规划问题转换成非线性优化问题,代表方法有打靶法、直接配点法、迭代线性二次调节器(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所示,图中:
图1
建立三维物体曲面
式中:ρ为球半径;0<u<π,-π<v<π.
基于正交坐标系,定义曲面上某一点f(U)处的单位高斯坐标系为
1.2 滚动约束运动方程
两个三维刚体滚动接触运动模型如图2所示,考虑空间物体1受到与物体2的距离为
图2
根据文献[16]的结果,2个三维物体纯滚动的一阶运动学方程为
式中:
同时,保持两物体接触的约束条件为
两物体间纯滚运动的约束条件为
式(3)可表示为如下的控制仿射形式:
式中:
2 滚动约束运动规划算法
2.1 问题的描述
非完整系统的运动规划问题可以转换为最优控制问题:寻找满足非完整约束式(6)的一个最优
式中:qstart为起点的位形;qgoal为终点的位形.
同时使如下二次型性能指标函数为极小:
式中:
除起终点约束外,对于三维空间流形,需要解决奇异点的问题,一种可行的方法是用多个坐标映射构建一个图册.为简化问题,通过引入不等式约束避免奇异点,例如对式(1)中球面参数坐标系在极点位置
同时,对两物体的相对角速度幅值c建立如下约束:
式中:cmin为最小控制输入向量;cmax为最大控制输入向量.
为简化表达,使用ξ(t)代表(q(t),c(t)),上述最优控制问题可表示为如下形式.
输入:物体曲面模型、qstart及qgoal.
输出:满足式(8)、(12)及(13)约束且使性能指标函数式(9)最小的最优c*(t)(t∈[0,T]),即
式中:
2.2 运动规划算法
针对上述建立的最优控制问题,建立如图3所示的在线控制算法框图,实现系统的在线控制,图中c1为名义控制输入.
图3
第一阶段为离线运动规划,使用配点法快速获得名义轨迹.将运动规划过程分为
首先利用插值函数计算区间中点处的状态变量
式中:c0~c3为多项式系数.
同时在区间端点处满足:
式中:
求解式(15)和(16)构成的线性方程组,可以获得
使用辛普森积分计算性能指标函数:
在每个子区间上使用辛普森积分计算非完整约束:
式(18)中
因此,式(14)表达的系统离线运动规划问题可以表示为如下的非线性规划问题:
针对上述非线性规划问题,使用与目标轨迹的距离作为性能指标,为获得轨迹最短的优化结果,使用初始点和目标点间的线性插值作为配点法的目标轨迹.
在使用配点法优化轨迹时,迭代初值的选取对最终的求解结果有很大影响,在实际应用中,常对5个状态变量轨迹有不同要求,例如球形机器人在平面滚动,期望平面上的接触点轨迹最短,而对球面接触点轨迹和物体间相对转角轨迹要求满足起终点约束即可,应选择使得平面上接触点轨迹为直线的控制输入作为配点法的初始值.图3中两状态控制法的目的即是通过控制5个系统状态中两个状态轨迹为起终点间线性插值,使用式(3)反向求解获得对应的控制输入ctc(t).具体实现方法如下:
将运动规划整个过程分为
式中:[1]、[2]分别表示提取q的第1、第2个元素.
由式(3)的第1个方程可得:
式中:
由式(3)的其余两个方程,使用欧拉法得到其余3个状态变量轨迹的计算公式为
第二阶段为在线运动规划,使用离线规划的结果
首先将非线性模型式(6)线性化:
式中:
在预测区间内,最优控制律可以表达为
对应的系统状态变量求解形式为
式中:
构建预测区间内运动规划的二次型函数J1:
步骤1 求解最优控制
式中:
式中:
式中:
推导出最优控制
步骤2 求解最优控制介入时间
式中:
步骤3 通过线搜索迭代求解最优控制时长λ:
式中:
3 仿真计算及结果分析
3.1 仿真参数
基于上述建立的滚动约束系统在线规划算法,通过MATLAB软件实现球-平面模型和球-球模型的仿真,仿真程序运行环境为:i7-4770MQ CPU@3.40 GHz 12 GB RAM.规划周期均设置为1 s,使用序列二次规划(SQP)算法实现非线性规划问题式(20)的求解,获得参考轨迹和名义控制输入,在实时运动规划过程中应用四阶龙格库塔法求解非完整约束式(7).
离线规划中配点法使用表1中的参数,配置41个配点,设置相应的控制变量幅值约束,对
表1 滚动接触模型离线运动规划参数
Tab.1
参数名 | 参数值 |
---|---|
配点数 | 41 |
控制变量约束 | |
Q | diag([100 100 10 10 1]) |
R | diag([0.1 0.1]) |
P1 | diag([20 20 10 10 1]) |
使用表2中的参数作为在线运动规划的仿真参数,模拟反馈控制频率为50 Hz,设置预测时间窗口为0.5 s,求解式(7)预测模型运动,并将模拟结果作为传感器的反馈输入.表中kmax为迭代次数的最大值.
表2 滚动接触模型在线运动规划参数
Tab.2
参数名 | 参数值 |
---|---|
传感器采样频率 | fs=50 Hz,ts=0.02 s |
预测区间/控制时长初值 | Tp=0.5 s,Δtinit=0.1 |
目标函数下降上限系数 | γ=-5 |
线搜索参数 | ω=0.55,kmax=6 |
Q | diag([1000 1000 50 50 1]) |
R | diag([0.1 0.1]) |
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时刻的状态,并标注两个物体上接触点的最终优化轨迹.对比起终点位置处球模型上固连坐标系的状态,证明球面接触点的位置
图4
图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
图5
球-平面模型优化控制输入和状态空间曲线
Fig.5
Optimized control input and state space curves of sphere-plane model
图6所示为球-平面模型的每步的计算时间tc与仿真时间(即前文控制时间)的关系,在该案例中,计算时间为6.5~8.9 ms,单步平均求解时长为7.1 ms.在仿真中,采样时间为20 ms,考虑到使用MATLAB软件计算效率较其他编译型语言低,建立的控制器可用于中低速运动的场景.
图6
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上的目标位置.
图7
图7
球-球滚动接触仿运动规划结果
Fig.7
Planning results of sphere-sphere rolling contact simulation motion
图8所示为球-球模型仿真结果,5个位形坐标在t=1 s时刻达到预定的目标位置,由于对物体1上接触点参考轨迹设置的权重较大,在线规划的最终轨迹与物体1上的参考轨迹有较高的重合度.
图8
图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
4 结语
本文基于纯滚动接触运动学模型,构建了含滚动约束这一类非完整系统的在线运动规划算法框架.针对已有的研究成果受限于特定的模型且提出的算法只适用于离线的、全局的运动规划的问题,改进了SAC算法,采用离线运动规划结合在线运动规划的方式,建立了通用性更强的在线运动规划算法.通过配点法快速获得低精确度的参考轨迹,在实时规划过程中通过反馈校正和滚动优化获得下一步的优化控制输入.将算法用于球-平面模型和球-球模型的仿真,结果证明该算法可用于解决球形机器人地面运动控制和机械手指操作物体的实时运动规划问题,也为其他滚动接触机器人系统的控制算法提供了可行的解决方案.
参考文献
非完整约束下的机器人运动规划算法
[J].
Motion planning for robot with nonholonomic constraints
[J].
求解最优月球软着陆轨道的隐式打靶法
[J].
Implicit shooting method to solve optimal Lunar soft landing trajectory
[J].
小行星探测器轨迹优化方法
[J].
Optimization for asteroids spacecraft trajectory
[J].
Trajectory planning and optimized adaptive control for a class of wheeled inverted pendulum vehicle models
[J].DOI:10.1109/TSMCB.2012.2198813 URL [本文引用: 1]
Sequential action control: Closed-form optimal control for nonlinear and nonsmooth systems
[J].DOI:10.1109/TRO.2016.2596768 URL [本文引用: 1]
复杂环境下基于RRT的智能车辆运动规划算法
[J].
RRT-based motion planning algorithm for intelligent vehicle in complex environments
[J].
A novel RRT extend function for efficient and smooth mobile robot motion planning
[C]//2014 IEEE/RSJ International Conference on Intelligent Robots and Systems.
A framework for the stabilization of general nonholonomic systems with an application to the plate-ball mechanism
[J].DOI:10.1109/TRO.2004.839231 URL [本文引用: 2]
Motion of two rigid bodies with rolling constraint
[J].DOI:10.1109/70.88118 URL [本文引用: 2]
A motion-planning algorithm for the rolling-body problem
[J].DOI:10.1109/TRO.2010.2053733 URL [本文引用: 2]
空间物体点接触纯滚动的几何意义
[J].
On nonholonomic constraints about the pure rolling of point contact
[J].
Global formulation and motion planning for a sphere rolling on a smooth surface
[J].DOI:10.1007/s12555-018-0011-3 URL [本文引用: 1]
Rolling sphere problems on spaces of constant curvature
[J].DOI:10.1017/S0305004108001084 URL [本文引用: 1]
Optimal control of the sphere S n rolling on En
[J].DOI:10.1007/s00498-004-0143-2 URL [本文引用: 1]
The kinematics of contact and grasp
[J].DOI:10.1177/027836498800700302 URL [本文引用: 1]
Control theory from the geometric viewpoint
[M].
/
〈 |
|
〉 |
