徐 帷,卢 山
(1. 上海航天控制技术研究所,上海 201109;2. 上海市空间智能控制技术重点实验室,上海 201109)
近年来,航天任务的日益发展对航天器的结构和功能提出了大型化和复杂化等要求,这使得在轨服务与操作技术在诸如航天器在轨维修、模块更换甚至大型航天器在轨建造等方向具有极大的应用前景,但同时也存在操作的安全性和稳定性方面的较大挑战。传统的研究思路是借助于操作航天器上的多源相对状态敏感器,由航天员或地面人员遥操作决策,对捕获机构、飞网或舱外机械臂等执行机构输出相应指令以实现对相对状态的控制[1]。由于存在信号传输的天地大回路时延、不同人决策经验的相异性等因素,这种完全由人参与的在轨操作都有局限性。因此,国外最新研究主要是针对在轨操作的自主、智能化任务规划和决策等方向开展的[2]。
空间机械臂由于可利用冗余自由度来同时实现对目标的捕获和障碍物回避等复杂操作,从而易于实现在轨的精细化操作,因此和其他执行机构相比具有较大的应用前景。为实现对其的平稳控制,相关学者在其路径规划方面进行了深入研究。陈钢等[3]利用一种主任务零空间的重复运动规划方法对关节角速度进行规划,减小了关节角与基座位姿在运动过程中的漂移。黄兴宏等[4]将轨迹规划问题转换为最优控制问题,设计了一种全程恒定基座姿态零扰动的末端位姿轨迹规划方法。以上文献[3-4]均未涉及障碍物避碰问题。王悦等[5]提出一种试探性规划算法,通过碰撞检测、无碰撞目标构型求解、无碰撞路径搜索算法和路径平滑等步骤,在C空间中搜索出一条能使机械臂不与障碍发生碰撞且末端到达目标点的路径,但该方法只适用于静态障碍物环境,且在平滑曲线时需要人工干预以合理地划分运动段。文献[6]基于非线性模型预测理论提出了一种避障方法,但臂杆到障碍距离的约束构造存在一定困难。Montiel等[7]采用一种人工势场法,实现了复杂动态环境下的避障路径规划,但是此方法是根据局部信息计算获得规划结果,容易产生局部极小值而使得机械臂在障碍物附近反复运动造成任务失败。羊帆等[8]基于障碍物伪距离技术,采用逆几何模型求解期望末端位姿下的连杆伪距离估计值构造避障约束条件,进而通过求解非线性优化问题,获得避障期望轨迹。
上述传统思路大部分可总结为在特定的本体动力学、目标及障碍特性情况下对最优规划策略的研究。当存在更复杂的环境未知且不确定情况时,这些方法的鲁棒性和可行性都面临一定疑问。强化学习是机器学习领域的一个重要分支,智能体通过与环境的交互,在多次试错过程中接收从环境获得的评价反馈,逐渐学习得到不同环境状态下的最优行为策略[9-10]。由于满足马尔科夫过程的强化学习无需环境精确模型和先验知识,而仅需感知环境状态,就能完成从环境状态到动作映射的学习,且其在线运行时还能针对动态变化的环境进行策略的实时修正,因此能够克服传统决策方法的不足,在机器人决策与控制领域已经得到一定应用[11-13]。
借鉴上述相关研究成果,本文以空间机械臂末端目标跟踪捕获及全臂的避障任务为研究背景,在对机械臂-目标-障碍进行三维空间运动学关系分析的基础上,设计符合强化学习应用要求的二维状态变量和拟合奖赏函数,利用结合资格迹的Sarsa(λ)强化学习算法进行各臂转动动作的强化训练,仿真结果表明由本算法得到的各臂的最终状态-动作值函数表可作为机械臂在线路径规划的决策依据,该表后期在线自主调整能力亦较强,因而验证了本算法较强的鲁棒性和智能性。
图1 强化学习基本框架Fig.1 Basic frame of reinforcement learning
强化学习一般需要对随机与离散状态这一类问题进行建模。在实际应用中,最常采用的是马尔可夫决策过程(MDP)。现实中MDP四元组E=S,A,P,R往往很难得知,该情形下的强化学习称为免模型学习[14]。与批处理式的、通过多次采样求取平均积累奖赏来作为期望积累奖赏的Monte Carlo方法相比,时序差分方法在每次试探后都利用即时获得的奖赏信号对状态-动作值函数Q(s,a)进行增量式更新,因此具有较强的实时性和在线学习能力。
(1)
Agent的动作选择通常采用ε-贪心策略或者Softmax算法。这两种算法的基本思想都是确定状态动作值函数Q(si,ai)到动作ai在状态si下被选择概率的映射。这两种策略表示如下:
πε-greedy(s,a)=
(2)
(3)
式中:A为动作集中包含的基本动作数量。
由式(2)可见,ε-贪心策略中最大的Q值对应的动作被选择的概率最大,其他动作的概率相等;而Softmax策略中各动作的概率依据其对应Q值不同形成Boltzmann分布,同时通过温度参数T可进一步调节各动作被“探索”和“利用”的随机水平。由于Softmax策略的灵活性,本文选择其作为空间机械臂规划训练时的动作选择策略。
式(1)表示的基本Sarsa算法与有效迹结合可获得学习收敛速度更快的Sarsa(λ)算法[15]。有效迹et可按下式进行递推表示:
et+1(s,a)=
(4)
式中:γ是折扣因子,而λ是衰减系数。由式(4)可见,某状态被访问且其下某动作被选择得越频繁,对应的资格迹衰减得越慢,因此有效迹可看做是状态-动作对(s,a)被访问的频次的一种体现。
Sarsa(λ)算法即是将有效迹加入基本Sarsa算法的值函数增量式更新部分:
(5)
工程中常见的空间机械臂具有六或七个自由度结构。图2所示为典型六自由度灵巧操作机械臂,分别由2个肩关节(A和B)、一个肘关节(C)以及三个腕关节(D、E和F)组成。
图2 空间多自由度机械臂分解结构Fig.2 Breakdown structure of multi DOF space manipulator
由图2可见,机械臂末端在工作空间中所能到达的位置由六个关节所转动的角度共同决定。但是,最末的两个关节E和F主要决定机械臂末端安装的操作器在本体系下的姿态;对于大范围运动目标跟踪这种粗捕获任务而言,对末端定位起主要作用的还是A~D四个关节自由度。因此,对于本文研究的路径规划和决策问题,可以将六自由度结构进行合理简化,用图3所示的模型进行描述。
图3中Oxyz为机械臂在平台上的安装基准坐标系。θ1~θ4分别为两个肩关节、一个肘关节a和一个腕关节b相对于其零位的转角,当这4个转角均为零时,机械臂初始零位沿x轴方向伸展。W为机械臂的工作平面(图3中虚线框所示),由肩关节一的实时转动角度θ1确定,而其后的各关节及对应三节臂杆(长度分别为l1、l2及l3)、末端点c均在W平面内运动。T为末端点c期望跟踪的目标点,M为障碍物,其在W面内的旋转投影点(即T绕z轴旋转至到达W平面内的点)分别为T′和M′。
在作仿真分析时,图3所示的各点在运动过程中的空间坐标按如下方法计算:
(6)
点T′和T(xT,yT,zT)、点M′和M(xM,yM,zM)的关系为:
(7)
根据2.1节描述,环境状态s的设计需要能够体现末端点c对目标T的偏离以及各agent相对障碍M的距离。因此,这里设计由目标偏差和障碍距离组成的二维状态。设决策θ1~θ4转角变化量a1~a4的智能体编号为AG1~AG4。
对于AG1而言,其主要作用是让AG2~AG4所在的工作平面W能够到达T(即T=T′)。在该过程中,W有可能会多次经过M(即M=M′),但这往往是无法避免的,对M的回避主要由AG2~AG4的转动来实现。因此,AG1对应的目标偏差ΔT1和障碍距离ΔM1可分别定义为点T和T′以及M和M′相对于z轴的张角,即:
(8)
对于AG2~AG4而言,各关节转动只会对c在W面内的位置产生影响,当c到达T′时,才算完成面内目标跟踪的任务。因此,ΔT2~ΔT4表示为:
(9)
由式(9)可见,ΔTi(i=2,3,4)主要体现了c和T′对各关节的张角;在实际工程应用中,通过传感器感知该张角是容易实现的。
同时M与特定AGi(i=2,3,4)的距离只需考虑该臂杆线段范围而不是整条直线,因此可考虑采用椭圆形状的障碍距离程度评价,此时臂杆两端点为椭圆焦点,当M在运动过程中位于同一个椭圆上时,可认为其与臂杆的距离程度相等,发生碰撞的危险性相同。以AG2为例,上述障碍距离程度如图4所示,ΔM2-ΔM4按照式(10)进行计算。
图4 椭圆形障碍距离程度判据Fig.4 Elliptic criterion for degree of obstacle distance
(10)
需要说明的是,机械臂在实际工作环境中可能有多个障碍,实际应用中AGi只需选取感知到实时距离程度最近的障碍进行后续环境状态的判断。
将上述分析的目标偏差ΔTi和障碍距离ΔMi组成二维状态向量Si=ΔTi,ΔMi,Si即可作为AGi对自身所处完整环境的感知依据。由式(1)可知,Sarsa算法的实施需要对不同状态及动作分配存储空间,以记录值函数Q(sj,ak),因此为避免数据处理过程中的“维数灾”问题,这里首先对连续二维状态Si进行离散化划分。如图5所示,对于AG2~AG4,将ΔTi和ΔMi各分为6段(QS=Quite Small,S=Small,M=Middle,B=Big,VB=Very Big,QB=Quite Big;NB=Negative Big,NM=Negative Middle,NS=Negative Small,PS=Positive Small,PM=Positive Middle,PB=Positive Big),则二维状态空间被划分为36个区间(对每个区间对应的二维状态分别用两位数字进行编号)。如图6所示,AG1的状态划分类似,但ΔMi可为正负,所以原点位置有所不同。
图5 AG2~AG4二维状态空间S2~S4离散划分Fig.5 Discrete division of two-dimensional state space S2~S4 for AG2~AG4
图6 AG1二维状态空间S1离散划分Fig.6 Discrete division of two-dimensional state space S1 for AG1
基于上述分析,基于Sarsa(λ)强化学习的路径规划算法流程如下。
1)初始化:AGi下,对∀si∈Si,ai∈Ai,令Q(si,ai)=0,e(si,ai)=0,其中Q(si,ai)和e(si,ai)分别代表si所位于的离散状态区间下动作ai的值函数和有效迹;设定初始温度参数T0、试探次数Pi=0、学习周期数Ei=0;
2)AGi感知二维状态si(0),并依据式(3)按一定概率选择一个动作ai(0);
3)AGi执行所选择的动作ai(0);
4)AGi感知下一时刻状态si(1);Pi++,Ti=T0/Pi;并依据式(3)按一定概率选择动作ai(1);
5)根据环境转移状态si(1)从拟合奖赏函数反馈奖赏值Ri[ΔTi(1),ΔMi(1)];用式(5)更新Q[si(0),ai(0)]、e[si(0),ai(0)];
6)判断:若至少满足以下两条件之一,则转7);否则转8);
条件①:AGi与障碍物相碰;
条件②:末端点c到达目标附近;
7)Ei++,Pi=0;判断:若Ei达到设定上限值,则结束学习,否则依据式(3)重新选择动作ai(1),并转8)。
8)状态及动作更新:si(0)←si(1),ai(0)←ai(1),转3)。
本部分通过在Matlab R2014a环境下建立的模型进行仿真,验证上述路径规划各设计步骤的有效性和工程实用性。
机械臂结构及初始构型参数、障碍与目标相对于基准坐标系的运动情况及强化学习算法参数如表1~3所示,设ω1=0.03 rad/s、ω2=0.015 rad/s。用于拟合奖赏函数的离散二维状态点及对应的奖赏值如表4~5所示。
由表4可见,ΔMi愈大,ΔTi的绝对值愈小,则
表1 仿真初始参数Table 1 Simulation parameter
表2 目标运动参数Table 2 Target moving parameter
表4 AG2~AG4奖赏函数拟合数据Table 4 Data used for reward function fitting of AG2~AG4
表5 AG1奖赏函数拟合数据Table 5 Data used for reward function fitting of AG1
从环境反馈的奖赏值愈大,表示对实现目标跟踪及避障的奖励;反之,ΔMi愈小,ΔTi的绝对值愈大,则从环境反馈的奖赏值愈小,表示对状态背离任务目标的惩罚。表5亦可作类似的解读。
将表4和表5数据用双立方插值方法进行曲面拟合,可得到如图7和图8所示的连续二维状态空间上的光滑奖赏函数曲面。
图7 AG2~AG4拟合奖赏函数Fig.7 Fitted reward function for AG2~AG4
图8 AG1拟合奖赏函数Fig.8 Fitted reward function for AG1
图7和图8分别为ΔMi和ΔTi在最小到最大数据点范围内的拟合曲面,为保证算法能平稳收敛,二维状态空间所有区域上的奖赏值必须有界,这只需对图中ΔMi、ΔTi范围边界上的拟合值进行水平延展即可,此时拓展二维状态空间上的最大、最小值和图中的最大、最小值相等。
根据上述条件开展仿真分析,步长0.1 s,即每0.1 s执行一次Sarsa(λ)路径规划学习算法,仿真主要分为两阶段。第一阶段为离线训练阶段,即0~50 s内目标静止于初始位置,各臂杆智能体通过Sarsa(λ)强化学习算法寻找机械臂末端点c能够跟踪目标T的无碰撞联合构型。如图9所示,经反复探索,机械臂从初始沿x轴平行伸展的构型逐渐变换到末端点c稳定位于T附近且与障碍物无碰撞的构型,这主要是通过动作集A中大范围搜索动作(±5°、±2°)配合小范围调整动作(±0.88°、±0.31°)以及静止动作(0°)实现的。第二阶段为在线应用阶段,即50 s之后目标开始作表1所示的三轴空间运动,该阶段主要考察离线训练阶段对环境状态和相应动作策略学习的效果,模拟空间机械臂执行捕获连续运动目标同时避障的任务情况。如图10所示,机械臂随目标T的运动作相应构型调整,使得末端点c始终跟随T的运动。在机械臂工作平面W将要到达障碍M时,臂杆Oa及ab及时调整构型做了避让动作,同时c依然对T实现了稳定跟踪。
图9 离线训练过程Fig.9 Off line training process
图10 在线应用过程Fig.10 On line application process
图11 全过程ΔMi、ΔTi及cT′变化情况Fig.11 Curves of ΔMi, ΔTi and cT′ during entire process
如图11所示,各agent在两个仿真阶段都实现了任务目标。在离线训练阶段(0~50 s),ΔT1逐渐收敛到零附近,表示工作平面W及时到达了目标T。同时由于此时障碍M也恰好位于W内,所以ΔM1同期也减小到零。此时AG2~AG4各臂杆开始调整构型并做出避让动作,在50 s时基本实现避障及目标跟踪。在线应用阶段,ΔT1除了在ΔM1过零时间段附近有少量抖动外,其他时间都为零,表示工作平面W能跟踪运动目标T,以及W到达障碍M时需要根据实际环境作进一步学习判断;AG2~AG4的联合动作能够保证对M的避让同时末端点c稳定跟踪T′,在稳定跟踪阶段,规划精度达到ΔT1优于1°、cT′优于10(由于强化学习的特性,在线应用阶段在达到目的后仍然要在T附近进行试探,所以仿真曲线上体现出微幅振荡性,实际工程应用中可用平滑滤波方法进行处理。)
图12 全过程Pi及Ei变化情况Fig.12 Curves of Piand Ei during entire process
如图12所示,离线训练阶段由于要探索并形成初始策略,故第一个Ei下Pi增长较大,在初始策略形成后,以后只需根据环境的变化做少量的探索就可以保证对T的跟踪和M的避让。
离线训练阶段最终的二维状态-动作值函数如表6所示(以AG1为例)。
由表6可知,50 s离线训练结束时的状态-动作值函数表体现了经过探索试错形成的状态-动作映射策略。例如在状态S22下,动作-2°的值函数最大,即AG1选择让θ1转动-2°的概率最大。对照图6可知,状态S22对应于ΔM1和ΔT1均为Negative Middle,此时转角θ1确实需要中等程度地减小,才能使得工作平面尽快达到目标T,而选择动作-2°则恰好符合这种要求,其他状态下的动作值函数亦可作类似解读。以上分析验证了训练所得策略的正确性,因而可作为机械臂在线路径规划的决策依据。
表6 AG1部分状态-动作值离线训练结果Table 6 Off line training result of partial state-action value for AG1
针对自主化、智能化在轨精细操作的任务需求,本文将机器学习领域的强化学习思想应用于对空间多自由度机械臂的路径规划技术研究,设计了一种基于Sarsa(λ)算法的机械臂运动目标跟踪与避障方法,通过离线训练与在线应用全过程的数学仿真验证了算法的合理性。本方法应用过程不显含系统的动力学和运动学模型,也不像传统方法那样会因为雅可比矩阵求逆而容易出现奇异问题,因而具备一定的智能性与鲁棒性,以及工程化应用的可能性。实际工程中为便于在轨应用,可根据空间机械臂构型先期在地面进行如本文仿真校验部分所述的离线学习训练,利用所得的状态-动作值函数表作为执行在轨任务的初始策略依据,即可进行如本文仿真校验中在线应用阶段的直接运用,同时根据在轨环境特性的变化可实现策略的自主在线调整。另一方面,即使不进行地面先期离线训练,本文所述的方法也较适用于大型航天器的在轨制造/装配等任务。这些任务的共同特点是:环境特性未知,无法完全由人参与决策制定,但是环境在较长时间内具有慢变特性,同时留给机械臂进行精细操作的时间较充裕。这种情况下可充分进行在线强化学习训练,发挥本方法可实现自主、智能化任务规划和决策的优势。
本文后续进一步的研究方向包括提升动作输出平滑度的连续状态/动作空间的机械臂路径规划强化学习方法研究。