一种主动悬架式火星车稳定裕度优化控制策略

唐 玲，刘 涛，李德伦，魏世民

(1. 北京邮电大学自动化学院，北京 100876；2. 北京空间飞行器总体设计部，北京100094；3. 空间智能机器人系统技术与应用北京市重点实验室，北京 100094)

0 引 言

火星车是承载各种仪器执行火星表面探测任务的重要载体和直接工具。迄今为止，全世界共有4辆火星车成功完成在轨探测任务，分别为火星车旅居者、勇气号、机遇号和好奇号(Curiosity)[1-3]，4辆火星车均为摇臂式悬架机构。为解决火星车在火星表面的高性能移动问题,我国的火星探测采用了一种六轮主动悬架式火星车，在传统火星车的摇臂式悬架机构上加入了主动环节[4]。

行星车的稳定性反映了其在不同地形环境下的抗倾覆能力，是行星车移动系统的重要指标之一[5]。火星表面具有平原、山脉、丘陵、陨石坑等多种地形地貌特征，是一种典型的非结构化环境[6]。虽然火星车可以使用导航控制的方式在相对平坦的地形中行驶，但由于火星地形地貌复杂，陡峭坡面、地面凹凸不平、局部土壤沙化等非结构化环境均会引起火星车车体倾斜，火星车行驶过程中存在由于稳定性不足而导致倾覆的风险。因此，研究火星车在非结构化环境下的稳定性控制，对于火星探测任务的顺利开展具有重要的实际意义。

许多学者开展了对行星车稳定性分析和控制的研究。Kilitd等[7]提出了一种三悬架式火星车，建立其运动学模型，并以火星车运动过程中倾斜角度为评价标准，分析火星车的稳定性，但火星车的极限倾斜角度还与火星车的结构尺寸相关，因此该稳定性评价标准不具有普适性。Zhu等[8]人使用稳定锥法分析了一种轮腿混合式机器人爬升过程中的稳定性。邹怀武等[9]针对嫦娥三号巡视器在坡道转移过程中，以不出现车轮翘起为稳定准则，分析巡视器的行驶稳定性，而在非结构化环境中，当各车轮速度不匹配时，也会出现车轮翘起的情况，因此该稳定性准则具有一定局限性。文献[7-9]没有开展针对行星车稳定性的优化控制。Mcdermott等[10]通过控制一种四轮行星车车体位姿平衡以增强其行驶稳定性，但该稳定性控制方法不适用于爬坡工况。Nakamura等[11]基于稳定锥法，通过控制四轮式行星车质心位置，以提升其在崎岖地形行驶过程中的车轮驱动力以及抗倾覆能力，但该方法需要在行星车上加入一套移动装置才能实现整车的质心位置调整。Cordes等[12]针对一种三轮腿式行星车，以各个轮腿受力平衡为目标，调整悬架构型来实现行星车在崎岖地形上的平稳行驶。综上可知，行星车稳定性控制均是针对悬架构型可调整的行星车，而这些行星车的悬架构型与我国的主动悬架式火星车有一定的区别，这类稳定性控制方法在主动悬架式火星车上适用性有限。

目前应用于机械系统的稳定性指标主要有稳定裕度法和稳定锥法。稳定裕度是指质心投影点与由机构支撑点组成的边缘之间的最短距离[13]，稳定裕度相对于稳定锥法，物理意义明确，数学模型简单。稳定裕度法广泛应用于移动机器人领域，机器人通常以稳定裕度为标准，规划得出稳定裕度最大的行进路径[14-16]。主动悬架式行星车可以采用稳定裕度的方法，通过改变整车质心的位置，获得最优稳定裕度的悬架构型，来提高车辆行驶的稳定性。非结构化环境下行星车质心位置控制的关键是建立行星车行驶过程中的准确运动学模型。Tarokh等[17]考虑到车轮的旋转、纵向和横向滑动影响，建立了一个通用的火星车Rocky7运动学模型，但没有考虑火星车悬架机构构型调整的情况。Inotsume等[18]建立了一种四轮月球车的运动学模型，通过控制悬架机构构型来提升软土坡面上的行驶牵引力。高海波等[19]建立了一种主动悬架式火星车的运动学模型，分析火星车车轮抬离地面的性能。文献[17-19]中的运动学模型均没有考虑星球车侧倾时车轮与硬质地面的点接触情况。Lou等[20]建立六轮星球探测运动学模型，分析星球车整车运动速度与车轮速度分量之间的关系，但是运动学模型简略了悬架机构各个关节之间的运动关系。目前仍然缺乏足够完善的运动学模型来控制主动悬架式行星车悬架机构的构型。

针对以上问题，本文提出针对六轮主动悬架式火星车的稳定裕度优化控制方法。考虑火星车悬架机构调整过程中车轮与接触地面滑移和侧倾等运动关系，建立和完善六轮主动悬架式火星车的运动学模型，在此基础上，推导火星车的稳定裕度模型和火星车稳定裕度优化控制模型，拟通过调整火星车悬架机构构型，来实现火星车在非结构化环境下稳定裕度的优化控制。最后，采用Adams和Matlab/Simulink软件建立火星车动力学与控制联合仿真平台，对火星车的稳定裕度优化控制策略的有效性进行仿真校验。

1 六轮主动悬架式火星车运动学模型

六轮主动悬架式火星车结构图如图1所示。火星车由车厢、差速器、夹角调整机构、前主摇臂、后主摇臂、副摇臂、转向机构、车轮、车轮驱动机构和离合器组成。离合器和差速器是被动关节，其它机构是主动关节。主动悬架式火星车可以通过驱动机构控制主动关节来改变悬架机构构型，被动关节和连杆根据地形拓扑改变其关节角度。

图1 火星车结构示意图Fig.1 Structure diagram of the Mars rover

差速器左右两个旋转关节的运动约束关系为d1=-d2=d/2，其中，d1和d2分别Jd1和Jd2的转动角度，d是左侧差速器轴相对右侧差速器轴的转动角度。夹角调整机构的两个旋转关节运动约束关系为hfi=-hri/2，其中，hfi与hri为Jhfi和Jhri的旋转角度。ti,i=1,…,6为转向机构i的旋转角度wi,i=1,…,6是驱动关节转动角度。b1为左离合器i的旋转角度。

图2 火星车坐标系示意图Fig.2 Coordinate frames of the Mars rover

(1)

坐标系Oi相对于坐标系Oi-1的位置矢量与姿态变换矩阵可以表示为

(2)

(3)

坐标系Oi-1与坐标系Oi之间的齐次变换矩阵表示为

(4)

式中，Z13表示一个1×3的零向量矩阵。因此，火星车坐标系之间的相对关系可以通过式(4)得出。

δi=γi+βi

(5)

(6)

式中：r为车轮半径。

图3 车轮i轮地接触坐标系Fig.3 Coordinate frames for terrain contact at wheel i

使用运动学模型描述t-Δt时刻至t时刻火星车构型调整过程中坐标系之间的关系。Oi0表示为t-Δt时刻坐标系，Oi表示为t时刻坐标系。因此，Tci0,ci,i=1,…,6表示为Oci(t)=Oci相对于Oci(t-Δt)=Oci0的齐次变换矩阵。坐标系Oci0与Oci之间的关系简图如图4所示。因此，Tci0,ci可以表示为式(7)所示。

(7)

式中：ξi为车轮i滚动滑移，ηi为车轮i侧向滑移，ζi为车轮i倾斜角度。

图4 滑移与倾斜运动Fig.4 Incremental motion by rolling and slip

Tcm0,cm表示从t-Δt时刻至t时刻车厢质心坐标系Ocm的齐次变换矩阵，可以表示为

Tcm0,cm=Tcm0,ci0Tci0,ciTci,cm

(8)

以i=4为例，Tcm0,cm可以表示为

Tcm0,cm=Tcm0,c40Tc40,c4Tc4,cm

(9)

式(9)中Tcm0,c40为

(10)

式(9)中Tc4,cm为

(11)

2 火星车稳定裕度优化控制系统

2.1 火星车稳定裕度计算模型

为了便于分析稳定性，定义了全局坐标系Ot。在t时刻，Ot的坐标系原点与Ocm原点重合。Ot坐标系的x轴方向与重力方向平行且相反。z轴方向与水平面平行，并指向火星车车厢前板。y轴满足右手定则。火星车稳定裕度示意图如图5所示。OR为火星车整车质心坐标系，可以通过杆件质量和火星车构型得到，OR的相对位置不受构型调整的影响。因此，假定OR相对于Ocm的位置和姿态始终不变化。OR′是OR在水平面上的投影；Oci′是Oci,i=1,3,4,6在水平面上的投影。h13,h36,h14,h46表示OR′与由Oci′,i=1,3,4,6组成的边的距离。

图5 稳定裕度示意图Fig.5 Diagram of stability margin

Oc1,Oc3,OR相对于Ot的位置矢量可以分别表示为Pt,c1=Ct,cm0Ccm0,cmPcm,c1，Pt,c3=Ct,cm0Ccm0,cmPcm,c3，Pt,R=Ct,cm0Ccm0,cmPcm,R。Ct,cm0为坐标系Ocm0相对于全局坐标系Ot的旋转矩阵。Ct,cm0可以通过车厢陀螺仪获得。因此，三角形ΔOR′Oc1′Oc3′的三边矢量可以表示为:

(12)

三角形ΔOR′Oc1′Oc3′的面积S可以通过海伦公式获得

S=

(13)

其中

(14)

则稳定裕度h13可写作

(15)

稳定裕度h14,h36,h46的求解方法与h13相同，因此，火星车稳定裕度为

hm=min(h13,h14,h36,h46)

(16)

其中，h13，h14，h36，h46是关于火星车悬架机构初始和调整后各关节角度的函数。

2.2 火星车稳定裕度的优化控制模型

设ti0，hfj0，hrj0，dj0，bj0，δi0和ζi0是火星车运动学模型变量参数在t-Δt时刻的值。ti，hfj，hrj，dj，bj，δi，ζi，ξi和ηi是运动学变量参数在t时刻的值。根据式(15)和式(16)可知期望的稳定裕度hm是ti0，hfj0，hrj0，dj0，bj0，δi0，ζi0，ti，hfj，hrj，dj，bj，δi，ζi和ηi,i=1,…,6,j=1,2的函数。以下对每个变量进行分析和计算。

为了提高控制系统计算效率，在火星车稳定裕度优化控制模型推导过程中，对模型作以下简化：(1)假设火星车t-Δt时刻各车轮与地面为水平面接触，设置δi0=ζi0=0；(2)假设火星车构型调整过程中车轮没有滚动滑移和侧向滑移，设置ξi=ηi=0(i=1,…,6)；(3)设定构型调整过程中转向机构不参与构型调整，即ti=ti0(i=1,…,6)；(4)构型调整过程中差速器角度影响很小，设定d=d0。

此外，hfj和hrj变量参数为控制目标；ti0,hfj0,hrj0,dj0和bj0参数变量可以通过火星车传感器测量获得；δi,bj和ζi是火星车被动关节角度，是火星车夹角调整机构关节角度和地形参数的函数，需要使用火星车运动学模型推导获得。

根据上述关系得出式(17)所示的关系式。

(17)

通过式(17)，可推导等b1和δ1，如式(18)和(19)所示。

(18)

(19)

通过使用这些变量，火星车初始构型的坐标系Ob1,Oh1,Ot1,Oc1可以表示为

(20)

b2与δ2的求解方式和b1与δ1的求解方式相同。

图6 火星车非结构环境下构型调整示意图Fig.6 The configuration adjustment diagram of the rover

ζi,i=1,…,6表示在构型调节过程中火星车的侧倾角度，且各车轮的侧倾角度值均相同，如图7所示。ζi可以通过Oc1与Oc4的相对位置求得，如式(21)所示。

(21)

将火星车运动学模型变量参数ti0,hfj0,hrj0,dj0,bj0,δi0,ζi0,ti,hfj,hrj,dj,bj,δi,ζi,ηi,i=1,…,6,j=1,2的数值和表达式代入式(16)，可得出火星车期望的稳定裕度优化控制目标如式(22)所示。

图7 火星车侧倾角度Fig.7 Incline angle of the rover

(22)

针对式(22)的火星车稳定裕度优化控制目标函数，以火星车夹角调整机构角度hf1,hf2为变量，使用内点法求解，最终获得火星车最大稳定裕度hm及期望的夹角调整机构角度hf1d,hf2d。

2.3 火星车稳定裕度控制系统

火星车稳定裕度控制系统主要由火星车运动学计算模块、火星车稳定裕度计算模块、火星车稳定裕度优化控制模块、悬架机构主动关节路径规划模块，以及悬架机构主动关节PI控制器组成。火星车稳定裕度控制系统框架与流程如图8所示，图中，Ii,i=1,2为夹角调整机构的控制电流。

图8 火星车稳定裕度控制系统Fig.8 The frame of the stability margin control system of the rover

各模块的主要功能描述如下：

1)火星车运动学计算模块，用于计算火星车悬架机构支撑车轮i与地面的接触坐标系Oci，i=1,3,4,6相对于车厢质心坐标系Ocm的位姿矩阵Pcm,ci。

2)火星车稳定裕度计算模块，根据火星车传感器获取的数据信息计算火星车的稳定裕度值，可实时监测火星车行驶过程中的稳定性。

3)火星车稳定裕度优化控制模块，该模块为控制系统的核心，使用内点法求解式(22)的稳定裕度函数，计算输出火星车的最优稳定裕度对应的期望的夹角调整机构角度hf1d,hf2d。

4)悬架机构主动关节路径规划模块和悬架机构主动关节控制器采用常规的方法实现，其中悬架机构主动关节路径规划模块采用速度梯形法对夹角调整机构的角速度进行规划，输出夹角调整机构的期望角速度ωf1d,ωf2d；悬架机构关节角速度采用PI控制器进行控制。

由于六角系统独特的图结构,因此研究它的点可区别边染色还是有一定难度的,文中所研究的图为“长”为n个相邻六边形,“宽”为m个相邻六边形构成的形为平行四边形的六角系统,并记作H(m,n),如图1所示。当m=1时,首先使用π(H(1,n))+1种颜色通过染色算法对H(1,n)(n≥6)中所有2度点进行边着色。其次,通过分析3度点的色集合,对个别边进行调色,证明了H(1,n)是满足点可区别边染色猜想的。当m≥2时,用k(关于m,n的函数)种颜色通过算法对H(m,n)进行边着色,并对相关边的颜色进行局部调整，从而给出关于H(m,n)的点可区别边色数的一个上界。

3 仿真校验

3.1 火星车动力学与控制仿真模型说明

为了验证火星车稳定裕度优化控制算法的有效性，本文分别使用Adams和Matlab/Simulink软件建立火星车高精度的动力学和控制模型，并在Matlab/Simulink软件中对火星车单侧轮越障、爬坡、坡面越障三种典型工况进行动力学与控制系统的联合仿真。其中，在火星车单侧轮越障工况中，车厢会发生侧向倾斜；在火星车爬坡工况中，车厢会发生纵向倾斜；在火星车坡面越障工况中，车厢会发生侧向倾斜并侧翻。

火星车典型工况的动力学模型主要包括火星车的多柔体动力学模型和非结构化地形模型。火星车动力学模型的主要参数设置为：火星车的长度为1600 mm，宽为1300 mm，高为800 mm，总重量为240 kg；在初始时刻，火星车整车质心位置Pcm,R=(-51.58，-13.80，-14.73) mm；火星车车轮采用运动模式(MOTION)驱动，角速度为6 °/s。火星车稳定裕度优化控制系统的主要参数设置为：火星车夹角调整机构采用PI控制器控制关节角速度，控制周期为10 ms，关节角度运动范围为[-π/4，π/4]；火星车稳定裕度优化控制模块控制周期为5 s。

3.2 火星车单侧车轮越障工况仿真与分析

火星车单侧车轮越障过程为：火星车左侧三个车轮依次从水平地面越过障碍，右侧车轮始终在水平地面行驶。单侧障碍物高度为296 mm，障碍前后坡度均为20°火星车与地面摩擦系数设置为0.3。分别对使用和未使用稳定裕度优化控制算法两种情况进行仿真，仿真时间为385 s，仿真步长为0.05 s。

仿真结果如图9和图10所示。图9为使用稳定裕度优化控制算法时，火星车越障过程中夹角调整机构关节角度hf1与hf2的时间历程曲线。未使用稳定裕度优化控制算法时夹角调整机构不运动，角度始终为0 rad，以下夹角调整机构转动角度曲线图中均省略该曲线。图10为使用和未使用稳定裕度优化控制算法两种情况下，火星车越障过程中稳定裕度的时间历程曲线。

图9 火星车单侧车轮越障过程中hf1和hf2曲线Fig.9 Curve of hf1 and hf2 during Mars rover one-sidewheels crossing obstacle

图10 火星车单侧车轮越障过程中稳定裕度曲线Fig.10 Stability margin curve of Mars rover during Marsrover one-side wheels crossing obstacle

从图9可以看出，使用稳定裕度优化控制算法时，在0～190 s火星车逐渐爬上障碍，hf1转动角度从初始状态0 rad逐渐增加至0.22 rad，hf2转动角度从初始状态0 rad逐渐减小至-0.58 rad，使得火星车左侧悬架机构的高度降低，右侧悬架机构的高度升高，减小了由于障碍物引起的车厢倾斜。375 s后，火星车驶入水平地面，夹角调整架构关节运动至初始角度0 rad，火星车悬架机构恢复为常规构型。

从图10可以看出，未使用稳定裕度优化控制算法情况下，火星车在水平地面时，稳定裕度初始值为600 mm；之后，火星车左侧车轮上坡，稳定裕度逐渐减小，火星车左侧三个车轮均在障碍物平面上行驶时，稳定裕度保持445 mm左右不变；之后，火星车左侧车轮下坡，稳定裕度逐渐升高，直至增加初始值600 mm。使用稳定裕度优化控制算法，整个运动过程中稳定裕度最小值为589 mm。在该过程中，使用稳定裕度优化控制算法相对于未使用稳定裕度优化控制算法，火星车的稳定裕度提高了32.35%。

3.3 火星车爬坡工况仿真与分析

火星车爬坡过程为：火星车从高度为0 mm的地面爬上30°坡，之后驶入高度为993 mm的地面。火星车与地面摩擦系数设置为0.6。分别对使用和未使用稳定裕度优化控制算法两种情况进行仿真，仿真时间为285 s，仿真步长为0.05 s。

仿真结果如图11和图12所示。图11为使用稳定裕度优化控制算法时，火星车爬坡过程中夹角调整机构关节角度hf1与hf2的时间历程曲线。图12为使用和未使用稳定裕度优化控制算法两种情况下，火星车爬坡过程中稳定裕度的时间历程曲线。

图11 火星车爬坡过程中hf1和hf2曲线Fig.11 Curve of hf1 and hf2 during Mars rover climbing

图12 火星车爬坡过程中稳定裕度曲线Fig.12 Stability margin curve of Mars rover duringMars rover climbing

从图12可以看出，未使用稳定裕度优化控制算法情况下，火星车初始时刻位于水平地面，稳定裕度为600 mm；之后，随着火星车驶入坡面，稳定裕度逐降低至441 mm；当火星车再次驶入高度为993 mm平地后，稳定裕度又逐渐增加至600 mm。使用稳定裕度优化控制算法时，火星车稳定裕度最小值为529 mm。在火星车爬坡过程中，使用稳定裕度优化控制算法相对于未使用稳定裕度优化控制算法，火星车的稳定裕度提高了19.95%。

3.4 火星车坡面越障工况仿真与分析

火星车坡面越障过程为：火星车行驶在30°的坡面上，火星车左侧三个车轮依次越过障碍，右侧车轮始终在坡面行驶。单侧障碍物高度为500 mm，障碍前后坡度均为20°火星车与地面摩擦系数设置为0.8。分别对使用和未使用稳定裕度优化控制算法两种情况进行仿真，仿真时间为550 s，仿真步长为0.05 s。

仿真结果如图13和图14所示。图13为使用稳定裕度优化控制算法时，火星车越障过程中夹角调整机构关节角度hf1与hf2的时间历程曲线。图14为使用和未使用稳定裕度优化控制算法两种情况下，火星车越障过程中稳定裕度的时间历程曲线。

图13 火星车坡面越障过程中hf1和hf2曲线Fig.13 Curve of hf1 and hf2 during Marsrover crossing obstacle on slope

图14 火星车坡面越障过程中稳定裕度曲线Fig.14 Stability margin curve of Mars rover during Marsrover crossing obstacle on slope

从图13可以看出，使用稳定裕度优化控制算法时，初始时刻火星车位于30°的坡面上，火星车开始调节姿态以增加其稳定裕度，hf1转动角度从初始状态0 rad逐渐增加至0.78 rad，hf2转动角度从初始状态0 rad逐渐减小至-0.78 rad，使得火星车左侧悬架机构的高度降低，右侧悬架机构的高度升高，减小了由于坡面引起的车厢倾斜。41 s后，火星车开始进行单侧越障，由于夹角调整机构转动角度已达到可调节最大值，因此，火星车悬架机构继续保持当前构型行驶。

从图14可以看出，未使用稳定裕度优化控制算法情况下，火星车在坡面时，稳定裕度初始值为244 mm；之后，火星车左侧车轮上坡，稳定裕度逐渐减小，在149 s时，稳定裕度降低至0 mm，火星车发生侧翻。使用稳定裕度优化控制算法时，0～41 s整车稳定裕度逐渐升高，41～234 s间，火星车夹角调整机构角度达到运动边界值，不发生变化，火星车稳定裕度逐渐降低，234 s左侧车轮全部位于障碍物上，稳定裕度达到最低161mm。273 s开始，左侧车轮依次离开障碍物，稳定裕度逐渐升高。

通过仿真分析可知，火星车稳定裕度优化控制算法可有效提高火星车行驶过程中的稳定裕度，以及爬坡和越障能力。相对于文献[10-12]中采用的稳定性控制方法，本文采用的控制方法适用于单侧障碍、坡面、坡面障碍等多种的非结构化环境，适用性更加广泛。

4 结 论

针对火星车在非结构化环境行驶过程中可能存在稳定性不足而引起倾覆的问题，提出了一种基于悬架机构构型调整的火星车稳定裕度优化控制方法。得出以下结论：

1)考虑火星车车轮与地面的滚动滑移、侧向滑移、车轮倾斜角度等因素，建立并完善了六轮主动悬架式火星车行驶的运动学模型。

2)推导得出非结构环境下的主动悬架式火星车稳定裕度的模型，可应用于火星车在非结构化环境行驶过程中稳定裕度实时计算和监测。

3)推导了火星车稳定裕度优化控制模型，并对控制模型做了有效简化。通过仿真与分析得出：该控制模型适用于火星车行驶的多种非结构化环境，并有效提高了火星车的行驶稳定裕度，以及爬坡和越障能力。在火星车单侧轮越障和爬坡两种典型工况中，使用稳定裕度优化控制算法，火星车稳定裕度分别提高了32.35%和19.95%；在坡面越障工况中，未使用稳定性优化算法，火星车产生了侧翻，使用稳定性优化算法后，火星车平稳地完成了坡面越障工况的行驶。