复杂环境下六足机器人的人机共融智能运动规划方法

1.本发明属于机器人技术领域。


背景技术：

2.足式机器人以其灵活的运动性能常被人们期许于能应用于复杂环境，如救灾抢险、野外运输、军事作战、星球探测等场景。且其离散落足优势使得其能够较好的适应于几何和物理特征存在突变的地形环境，具备轮式机器人所不具备的突出优势。特别地，六足机器人相对于四足、双足机器人来说，具备稳定性好、负载能力强、控制可靠，地形适应能力强的特点，具备极强的在复杂场景中广泛应用的潜力。
3.然而，要想充分发挥六足机器人的潜力，现实世界中复杂多变的环境对其自主感知、自主决策、自主规划等自主智能方面提出了很高的要求。受限于当前人工智能技术的发展水平，现有的自主智能算法的泛化性通用性还不强，达不到完全不出错的程度，自主系统极易受到环境意外变化的影响，尤其是当环境较为恶劣，不确定性增大时，对感知和决策系统都有着极大的挑战。


技术实现要素：

4.本发明目的是为了解决六足机器人对复杂环境的适应能力差的问题，提出了一种复杂环境下六足机器人的人机共融智能运动规划方法。
5.本发明所述的复杂环境下六足机器人的人机共融智能运动规划方法，具体包括：
6.步骤一、根据所需探测环境地图及目标位置，建立导引路径；
7.步骤二、采用三次均匀b样条算法对导引路径进行平滑，获取平滑的b样条曲线路径；
8.步骤三、计算平滑的b样条曲线路径的环境约束代价和曲线特性约束代价，对b样条曲线路径进行优化，获取优化后的行走路径；
9.步骤四、根据六足机器人的离散接触状态方程和优化后的行走路径，随机生成多个状态序列，并采用单步可达算法判断所有状态序列的可达性，若是存在可达的状态序列，执行步骤五，否则，重新建立引导路径，返回执行步骤二；
10.步骤五，采用深度优先剪枝搜索算法在可达的状态序列中搜索获取存在冗余的主状态序列；
11.步骤六、通过分治搜索算法对所述主状态序列进行优化，实现对六足机器人的运动规划。
12.进一步地，本发明中，步骤一中，根据所需探测环境地图及目标位置，建立导引路径的方法包括：两种方法，第一种方法：
13.首先确定目标位置p，通过快速扩展随机树算法生成一条初步避障路径，利用公式：
[0014][0015]
计算获取机器人的前进方向α和前进距离进而获得一条导引路径；
[0016]
第二种方法：确定目标点位置p，采用人为手绘的方式获取一初步条路径，并利用公式：
[0017]
v＝kmd0
[0018]
获取机器人的速度矢量和位移矢量；获取一条导引路径，式中，d0为把手端部相对于初始点的位移矢量，为虚拟控制对象的速度矢量，km为比例系数。
[0019]
进一步地，本发明中，步骤二中，获取平滑的b样条曲线路径的具体方法为：
[0020]
利用标准b样条方程：
[0021][0022]
对引导路径进行平滑，获取平滑的b样条曲线路径，式中,di为第i个控制点，n
i,k
为k次正则b样条的基函数，n为控制点的个数，u代表一维的状态参数，p(u)代表该状态参数u下对应的机器人状态。
[0023]
进一步地，本发明中，步骤三中，
[0024]
环境约束代价包括：障碍物约束代价；
[0025]
曲线特性约束代价包括：几何可行落足点密度约束代价，落足点物理特征约束代价，路径平滑约束代价和最短路径约束代价。
[0026]
进一步地，本发明中，步骤三中，对b样条曲线路径进行优化的具体方法方法为：
[0027]
建立考虑多约束条件的优化问题模型：
[0028]
min∑q
(.)
(θ)
[0029][0030]
采用梯度下降法对考虑多约束条件的优化问题模型求解，计算获取满足代价函数最小的路径，实现对b样条曲线路径进优化；式中，i代表第i个离散点，n为路径离散点数量，q(.)为障碍物约束代价、几何可行落足点密度约束代价、落足点物理特征约束代价、路径平滑约束代价和最短路径约束代价中任一代价遍历离散路径中所有点后的代价函数，q
(.)
(θi)为θi点的代价；θ为x的归一化离散路径点，x为六足机器人身体在笛卡尔空间下实际遵循的离散路径点序列。
[0031]
进一步地，本发明中，采用梯度下降法对考虑多约束条件的优化问题模型求解的过程为：
[0032]
步骤三一、对θ处朝着最优化方向的各分量的变化量进行估计：
[0033][0034][0035]
表示θ中第i个离散点θi处朝着最优化的变化量的估计，处朝着最优化的变化量的估计，表示以θ
为基准生成的第k条归一化的随机路径离散点序列；ε
θ,k
表示相对于θ的噪声序列，k表示参与当前迭代轮次估计的随机路径离散点序列数量；ε
θ,k
服从均值向量为0，协方差矩阵为r-1
的n维多元正态随机分布，其中，r采用公式：
[0036][0037]
确定，p
k,i
表示由中第i个归一化离散路径点的代价值所决定的概率值；p
k,i
采用公式：
[0038][0039]
确定，式中，表示第k个归一化随机路径离散点序列中第i个离散点；表示处的代价，其中，表示第m个归一化随机路径离散点序列中第i个离散点；λ为给定的参数，给定的参数，
[0040][0041]
对指数项归一化计算：
[0042][0043]
其中，h为常数；
[0044]
步骤三二、对θ处朝着最优化方向的各分量变化量的估计量进行平滑处理，获得最终的代价增量δθ：
[0045][0046]
其中，表示组成的各分量增量序列m表示平滑矩阵；所述平滑矩阵为：
[0047][0048]
步骤三三、令θ＝θ+δθ，返回步骤三一，直至考虑多约束条件的优化问题模型的总代价收敛，获得优化后的离散路径θ。
[0049]
进一步地，本发明中，步骤四中，六足机器人的离散接触状态方程为：
[0050]
φ＝《wrb，wrb，wpf，ff，σf》
[0051]
式中，φ为机器人的离散接触状态；wrb为当前状态机器人机体坐标系∑b相对于世界坐标系∑w的旋转矩阵；wrb为当前状态机器人机体坐标系相对于世界坐标系的位移；wpf为当前状态六个足端相对于世界坐标系的位移；ff为容错状态向量，反映当前状态六条腿的出错状态；σf为物理接触状态向量，反映当前状态下各足端与地面的物理接触情况。
[0052]
进一步地，本发明中，步骤四中，根据六足机器人的离散接触状态方程和优化后的行走路径，随机生成多个状态序列，并采用单步可达算法判断所有状态序列的可达性的具体方法为：
[0053]
步骤四一、计算每条状态序列的准静态稳定与运动学约束，判断每条状态序列的准静态稳定与运动学约束的逻辑状态是否为真，若是，则执行步骤四二，否则，该条状态序列不可达；
[0054]
步骤四二、计算状态序列的无碰撞约束，判断无碰撞约束的逻辑状态是否为真，若是，则执行步骤四三，否则，该条状态序列不可达；
[0055]
步骤四三、通过公式：
[0056]
s＝{j∈z:1≤j≤6,||d(j)||≤ε,σ
f,0
(j)＝σ
f,1
(i)＝1}
[0057]
|s|≥3
[0058]
计算状态序列中相邻两个状态的公共接触点数量是否大于等于3，若是，则两状态可达，执行步骤四四，否则，该条状态序列不可达；其中，j为机器人的第j条机械腿，d为相邻两状态足端位置的差值，d(j)为d的第j列，ε为给定的小阈值，σ
f,0
(j)和σ
f,1
(i)均为j号机械腿足端与地面的物理接触情况，σ
f,1
(i)表示σ
f,s
(j)在集合s中索引的值为1，σ
f,0
(j)表示在集合s中索引σ
f,s
(j)的值为0；
[0059]
步骤四四、判断机器人相邻两状态的离散接触状态方程的运动学约束状态是否为真，若是，则所述相邻两状态单步可达，否则，单步不可达。
[0060]
进一步地，本发明中，步骤五，采用深度优先剪枝搜索算法在可达的状态序列中搜索获取存在冗余的状态序列的具体方法为：
[0061]
步骤五一、根据优化后的行走路径和当前状态φ
t
确定前进方向vb；
[0062][0063]
其中，r
track
为路径的跟踪系数，δrb表示一段路径的微元，xi代表路径x上第i个待跟踪的离散点，r
b,t
表示t时刻机器人坐标系相对于世界坐标系的位移；
[0064]
步骤五二、根据当前状态φ
t
确定状态转移算子中支撑摆动状态向量cf的所有取值，并通过条件1和条件2对支撑摆动状态向量cf的所有取值进行筛选，获取支撑摆动状态向量cf取值集合sc；
[0065]
条件1：支撑摆动状态向量cf的某一位索引为1时，必须满足当前状态下各腿与地面的物理接触状态σf,t对应位索引为1且各腿的容错状态ff,t对应位索引为0，表示当某一机械腿要作为支撑腿时必须满足其与地面物理接触且不为容错腿；
[0066]
条件2：支撑摆动状态向量cf中索引为1的数量大于等于3；表示任一状态时都至少有3条机械足同时与地面接触；
[0067]
步骤五三、基于vb和sc初步确定与每一个状态φ
t
具有状态单步可达性的下一个状态φ
t+1
的子节点集合s
φ,t
；
[0068]
步骤五四、计算集合s
φ,t
中最大前进步长l
max
；
[0069]
l
max
＝argmax(||φ:r
b-φ
t
:rb||),φ∈s
φ,t
ꢀꢀꢀ
(0.37)
[0070]
其中，rb为当前状态机器人世界坐标系的位移，并判断所述最大前进步长l
max
是否小于给定步长阈值ε0，若是，则停止对下一个状态的搜索，完成存在冗余的状态序列的搜索；否则，执行步骤五五；
[0071]
步骤五五、判断当前状态的所有子节点的位置与优化后的行走路径的末端位置的距离是否小于极小阈值ε1；若是，完成存在冗余的状态序列的搜索；否则，执行步骤五六；
[0072]
步骤五六、判断搜索扩展的次数是否超出极限阈值次数tmax，若是，则完成存在冗余的状态序列的搜索，否则，返回执行步骤五三。
[0073]
进一步地，本发明中，步骤六中通过分治搜索算法对所述主状态序列进行优化的方法为：
[0074]
步骤六一：通过一个随机状态转移算子，考虑无碰撞和运动学等约束，生成趋向于终点的一系列单步可达的离散接触状态，形成一条随机序列；
[0075]
步骤六二：重复步骤六一，直至达到序列生成终止条件，获取a条随机序列；执行步骤六三；
[0076]
序列生成终止条件为：1、重复次数达到预设值；2、当随机序列的离散接触状态点聚集在一个状态点周围，不再继续前进，3、当随机序列的所有离散接触状态点与目标状态点的距离小于距离阈值；
[0077]
步骤六三：对所有状态节点进行状态间单步可达性判断，对单步可达的两个状态节点进行连接并标记，获取多个单步可达的状态节点；
[0078]
步骤六四：结合所述多个单步可达的状态节点，使用采用经典的贝尔曼-福特算法求解从起点状态到终点状态的最短状态序列，完成对所述主状态序列进行优化。
[0079]
本发明中路径规划包含两个步骤：备选路径集生成和路径选择，备选路径集生成由人机协同的方式进行；路径选择的整个框架从人类导引路径信息输入开始，直到算法将处理好的信息反馈给人形成一个人机协同的闭环，首先通过触觉设备人为给出参考的导引路径，然后进行路径平滑，对给出的导引路径进行初步的平滑，以消除导引路径的突变、抖动、不自然。接着进行路径优化，特异性的针对六足机器人，考虑复杂环境约束代价和曲线特性约束代价对路径进行进一步的优化。路径评估模块基于深度优先剪枝搜索的主序列生成算法快速生成一条存在状态冗余的非最优化状态序列，并在有限时间内通过分治搜索对其进行序列优化，尽可能逼近蒙特卡洛树搜索算法得到的最优结果，进而对实际跟踪序列做出规划。
附图说明
[0080]
图1是本发明所述方法人机协同框架的示意图；
[0081]
图2是为直线引导路径初始路径示意图；
[0082]
图3是由人机协同rrt生成引导路径初始路径示意图；
[0083]
图4是由人手直接绘制的引导路径初始路径示意图；
[0084]
图5是分治搜索主序列优化过程示意图；
[0085]
图6是实现本发明所述方法的软硬件架构示意图。
具体实施方式
[0086]
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动的前提下所获得的所有其他实施例，都属于本发明保护的范围。
[0087]
需要说明的是，在不冲突的情况下，本发明中的实施例及实施例中的特征可以相
互组合。
[0088]
具体实施方式一：下面结合图1说明本实施方式，本实施方式所述复杂环境下六足机器人的人机共融智能运动规划方法，具体包括：
[0089]
步骤一、根据所需探测环境地图及目标位置，建立导引路径；
[0090]
步骤二、采用三次均匀b样条算法对导引路径进行平滑，获取平滑的b样条曲线路径；
[0091]
步骤三、计算平滑的b样条曲线路径的环境约束代价和曲线特性约束代价，对b样条曲线路径进行优化，获取优化后的行走路径；
[0092]
步骤四、根据六足机器人的离散接触状态方程和优化后的行走路径，随机生成多个状态序列，并采用单步可达算法判断所有状态序列的可达性，若是存在可达的状态序列，执行步骤五，否则，重新建立引导路径，返回执行步骤二；
[0093]
步骤五，采用深度优先剪枝搜索算法在可达的状态序列中搜索获取存在冗余的主状态序列；
[0094]
步骤六、通过分治搜索算法对所述主状态序列进行优化，实现对六足机器人的运动规划。
[0095]
进一步地，本发明中，步骤一中，根据所需探测环境地图及目标位置，建立导引路径的方法包括：两种方法，第一种方法：
[0096]
首先确定目标位置p，通过快速扩展随机树算法生成一条初步避障路径，利用公式：
[0097][0098]
计算获取机器人的前进方向α和前进距离进而获得一条导引路径；
[0099]
第二种方法：确定目标点位置p，采用人为手绘的方式获取一初步条路径，并利用公式：
[0100]
v＝kmd0
[0101]
获取机器人的速度矢量；获取一条导引路径，式中，d0为把手端部相对于初始点的位移矢量，v为虚拟控制对象的速度矢量，km为比例系数。
[0102]
初始导引路径生成方法，有两个基本要求，一是对人类有较轻的认知负担，二是能够便捷直观的将人类意图转化为计算平台所理解的输入信息。基于上述考虑，对人类较为直观的方式是将六足机器人当成一个质点，借助主端机器人生成导引路径。在这个过程中，人类不需要有过重的心智负担，仅需按照意图图2提出的输入方式输入目标点和初始点进行初始路径输入即可。
[0103]
基于人类输入信息的导引路径生成的第一种输入方式是半自主的输入，通过人机协同的方式确定一条初始路径。人类仅需通过主端机器人设定一目标位置p，然后通过快速扩展随机树(rrt)算法快速生成一条粗略的可行避障路径，如图3所示。具体来说，操作员通过触觉设备的前进方向α和前进距离然后可计算得到目标位置p，其映射关系如式：
[0104]
[0105]
另一种输入方法是手动绘制路径。当自主规划算法规划的路径不能满足人类的需求时，例如控制机器人接近障碍区域或迫使机器人走特定路径，可完全手动输入方法如图4。虚拟控制对象的速度可以由触觉设备末端映射得到：
[0106]
v＝k
md[0107]
式中，d为把手端部相对于初始点的位移矢量，v为虚拟控制对象的速度矢量。
[0108]
进一步地，本发明中，步骤二中，获取平滑的b样条曲线路径的具体方法为：
[0109]
利用标准b样条方程：
[0110][0111]
对引导路径进行平滑，获取平滑的b样条曲线路径，式中,为第ib个控制点，为k次正则b样条的基函数，n为控制点的个数，u代表一维的状态参数，p(u)代表该状态参数u下对应的机器人状态。
[0112]
进一步地，本发明中，步骤三中，
[0113]
环境约束代价包括：障碍物约束代价；
[0114]
曲线特性约束代价包括：几何可行落足点密度约束代价，落足点物理特征约束代价，路径平滑约束代价和最短路径约束代价。
[0115]
本实施方式中，特别针对六足机器人面对极端稀疏落足点的复杂环境，提炼出如下几个代价：
[0116]
a)障碍物约束代价：将完全不可通行的区域比如超出六足机器人尺寸极限而无法跨越的物体、大坡度地形等处理为障碍物，然后建立障碍物地图的有符号梯度场(sdf)，sdf反映了点到障碍物边界的最近距离，障碍物外部的sdf值为正值，障碍物内部的sdf值为负值；其代价为：
[0117][0118]
这个代价保证了优化的路径朝着不与障碍物发生碰撞的趋势进行。该项代价值越小，机器人越不容易与障碍物发生碰撞，式中，其中xi为离散路径点序列中第i个元素，||x
i+1-xi||表示面对障碍物代价较大的地方应该更快的通过，六足机器人可被一个半径为r，球心在机器人中心的球包络，基于安全性的考量，留出安全裕度ε1，为障碍物约束代价函数，θi为路径第i个离散点。
[0119]
b)几何可行落足点密度约束代价：在对感知到的环境进行处理后，可以得到二值化的六足机器人可落足和不可落足的离散点，为了使六足机器人更容易通过极端稀疏的离散落足点地形，该项代价值越小，路径更加倾向于落足点密度较大的区域。进而有可行落足点密度代价如式：
[0120]qgf
(θi)＝k
gf
(n
s,max-ds(θi))
[0121]
式中，ds(θi)表示θi对应的xi周围面积为s的区域内离散可落足点数目，n
s,max
表示理论上对应区域内落足点数目的最大值；k
gf
表示可行落足点密度代价的权重。
[0122]
c)落足点物理特征约束代价：在野外环境中机器人不仅需要处理几何障碍物，还需要面对具有不同物理性质的地形，而不同物理性质的地形具有不同的通行代价，例如在
干燥的石板上行走的成本远高于在湿滑的冰上行走的成本。因此，规划的路径应该倾向于走较为安全的地形而避免到不安全可能造成失稳的地形附近。考虑了物理特征成本得到的代价公式为：
[0123][0124]
式中，k
j,slip
为归一化数值，表示地面材料切向的性质，k
j,fall
表示地面材料法向性质，k
pf
为环境物理特征代价的权重。j是路径第j个离散点θj的标号；
[0125]
d)路径平滑约束代价：为避免在优化过程中对路径的平滑性造成影响，这里引入平滑代价，定量由沿路径的加速度平方和表示；该项代价值越小，则路径曲线的加速度平方和越小，路径越光滑。
[0126]qsm
(θ)＝k
sm
θ
t
aa
t
θ
[0127]
式中，k
sm
为路径平滑代价的权重，θ为x的归一化离散路径点；
[0128][0129]
e)最短路径约束代价：实践表明，仅考虑上述代价而不考虑最短路径代价时会造成路径在面对复杂环境时的迭代过程中出现过分的缠绕与弯曲，进而使得路径特性不能达到预期效果。在此引入最短路径代价，与其他代价共同决定最终的生成路径。最短路径代价表示如式：
[0130][0131]
式中，k
sp
表示最短路径代价的权重，n为路径离散点数量，θn为离散路径第n个离散点，θj离散路径第j个离散点。
[0132]
进一步地，本发明中，步骤三中，对b样条曲线路径进行优化的具体方法方法为：
[0133]
建立考虑多约束条件的优化问题模型：
[0134]
min∑q
(.)
(θ)
[0135][0136]
采用梯度下降法对考虑多约束条件的优化问题模型求解，计算获取满足代价函数最小的路径，实现对b样条曲线路径进优化；式中，i代表第i个离散点θi的标号，n为路径离散点总数量，q(.)为障碍物约束代价、几何可行落足点密度约束代价、落足点物理特征约束代价、路径平滑约束代价和最短路径约束代价中任一代价遍历离散路径中所有点后的代价函数，q
(.)
(θi)为θi点的代价；θ为x的归一化离散路径点，x为六足机器人身体在笛卡尔空间下实际遵循的离散路径点序列。
[0137]
进一步地，本发明中，采用梯度下降法对考虑多约束条件的优化问题模型求解的过程为：
[0138]
步骤三一、对θ处朝着最优化方向的各分量的变化量进行估计：
[0139][0140][0141]
表示θ中第i个离散点θi处朝着最优化的变化量的估计，处朝着最优化的变化量的估计，表示以θ为基准生成的第k条归一化的随机路径离散点序列；ε
θ,k
表示相对于θ的噪声序列，k表示参与当前迭代轮次估计的随机路径离散点序列数量；ε
θk
服从均值向量为0，协方差矩阵为r-1
的n维多元正态随机分布，其中，r采用公式：
[0142][0143]
确定，p
k,i
表示由中第i个归一化离散路径点的代价值所决定的概率值；p
k,i
采用公式：
[0144][0145]
确定，式中，表示第k个归一化随机路径离散点序列中第i个离散点；表示处的总代价，其中，表示第m个归一化随机路径离散点序列中第i个离散点；λ为给定的参数，处的总代价：
[0146][0147]
其中，为处的障碍物约束代价，为几何可行落足点密度约束代价，为落足点物理特征约束代价，q
sm
(θ)为路径平滑约束代价，q
sp
(θ)为最短路径约束代价；
[0148]
对指数项归一化计算：
[0149][0150]
其中，h为常数；
[0151]
步骤三二、对θ处朝着最优化方向的各分量变化量的估计量进行平滑处理，获得最终的代价增量δθ：
[0152][0153]
其中，表示组成的各分量增量序列m表示平滑矩阵；所述平滑矩阵为：
[0154][0155]
步骤三三、令θ＝θ+δθ，返回步骤三一，直至考虑多约束条件的优化问题模型的总
代价收敛，获得优化后的离散路径θ。
[0156]
进一步地，本发明中，步骤四中，六足机器人的离散接触状态方程为：
[0157]
φ＝《wrb，wrb，wpf，ff，σf》
[0158]
式中，φ为机器人的离散接触状态；wrb为当前状态机器人机体坐标系∑b相对于世界坐标系∑w的旋转矩阵；wrb为当前状态机器人机体坐标系相对于世界坐标系的位移；wpf为当前状态六个足端相对于世界坐标系的位移；ff为容错状态向量，反映当前状态六条腿的出错状态；σf为物理接触状态向量，反映当前状态下各足端与地面的物理接触情况。
[0159]
进一步地，本发明中，步骤四中，根据六足机器人的离散接触状态方程和优化后的行走路径，随机生成多个状态序列，并采用单步可达算法判断所有状态序列的可达性的具体方法为：
[0160]
步骤四一、计算每条状态序列的准静态稳定与运动学约束，判断每条状态序列的准静态稳定与运动学约束的逻辑状态是否为真，若是，则执行步骤四二，否则，该条状态序列不可达；
[0161]
步骤四二、计算状态序列的无碰撞约束，判断无碰撞约束的逻辑状态是否为真，若是，则执行步骤四三，否则，该条状态序列不可达；
[0162]
步骤四三、通过公式：
[0163]
s＝{j
foot
∈z:1≤j
foot
≤6,||d(j
foot
)||≤ε2,σ
f,0
(j
foot
)＝σ
f,1
(i
foot
)＝1}
[0164]
|s|≥3
[0165]
计算状态序列中相邻两个状态的公共接触点数量s是否大于等于3，若是，则两状态可达，执行步骤四四，否则，该条状态序列不可达；其中，j
foot
为机器人的第j
foo
t条机械腿的索引，d为相邻两状态足端位置的差值，d(j
foot
)为d的第j
foot
列，ε2优选为0.4，σ
f,0
(j
foot
)和σ
f,1
(i
foot
)为j
foot
号和i
foot
号机械腿足端与地面的物理接触情况，σ
f,1
(if
oot
)表示σ
f,s
(i
foot
)在集合s中索引的值为1，σ
f,0
(j
foot
)表示在集合s中索引σ
f,s
(j
foot
)的值为0；
[0166]
步骤四四、判断机器人相邻的两状态是否均满足静稳态约束，若是则两状态单步可达否则，单步不可达。
[0167]
本实施方式中，进行多约束条件下状态单步可达性判断：
[0168]
a)无碰撞约束及其判别：六足机器人的运动规划过程需要接收上游算法模块给出的环境信息，包括无法跨越需要绕过的障碍物和用于支撑六足机器人运动的地面离散可落足点。记环境中障碍物点组成的集合为o
obs
，同时注意到在实际情况中障碍物往往是分块的独立障碍物，记第k个连通的障碍物点组成的集合为o
obsk
，有成立，k的取值范围由实际分块独立连通障碍物的个数k决定；离散可落足点组成的集合为o
fhold
。集合中各元素的值为该点在世界坐标系下的位置。
[0169]
基于上述符号定义，六足机器人的各腿在运动过程中不能发生碰撞，具体可建模为式：
[0170][0171]
式中，分别表示第i
foot
和第j
foot
条腿在角度配置为和情况下空间上的碰撞体积，除此之外，六足机器人各部分不能与环境发生碰撞。对六足机器人
整体的任意关节配置qa，表示空集，与环境不发生碰撞的无碰撞约束定义如式：
[0172][0173]
表示状态φ的无碰撞约束逻辑状态为真；
[0174]
当某一状态φ满足无碰撞约束可记作如式：
[0175]
collifree(φ)＝true
[0176]
在进行碰撞判别时，主要的思想是尽可能减少不必要检测的次数，能够自适应的根据实际机器人与环境障碍物的相对位置关系进行高效的判别。能够支撑这种判别特性的数据结构是自顶向下由大范围到局部细节的层次包围树。基于两个构建好的层次包围树进行碰撞判别时，有两个重点步骤需要关注，一是对两个层次包围树碰撞判别的遍历顺序，需要在判别不可能发生碰撞时提前剪枝退出，尽可能提高效率；二是对于在判别过程中树与树的节点间的碰撞判别应该尽可能快速、鲁棒。
[0177]
b)准静态稳定与运动学约束：给定一六足机器人状态φ，在该状态下六足机器人应该满足准静态稳定约束和运动学约束。对六足机器人状态φ而言，六条腿中表示与地面是否接触，能否受到地面作用力的足端组合为σf，该向量中腿编号索引值为1的，并且该编号索引的腿部出错状态ff对应值为0的腿编号构成集合k0，集合中元素个数为k0。则由牛顿-欧拉公式，稳定态有下式所示关系。
[0178][0179][0180]
式中，为第i
foot
号机械足在世界坐标系下的位置，底面作用到i
foot
号机械足端在世界坐标系下的力矢量，ob质心在世界坐标系下的位置，近似为六足机器人机体的几何中心，g为重力加速度，m为质量。
[0181]
考虑到六足机器人在行走过程中不应发生打滑，则地面作用到足端的力应始终保持在摩擦锥内，则有无碰撞约束的逻辑表达式成立：
[0182][0183]
式中：γ
1ifoot
为i
foot
号腿足端与地面接触的一个切向方向，γ
2ifoot
为i
foot
号腿足端与地面接触的另一个切向方向，n
ifoot
为i
foot
号腿足端与地面接触的法向方向，μ
ifoot
为i
foot
号腿足端与地面接触的摩擦系数，对六足机器人状态φ而言，需要满足运动学约束，表示状态φ的准静态稳定与运动学约束逻辑状态为真，即为表达式：
[0184]
ki(φ)＝true
[0185]
就本质而言，运动学约束是需要让任意腿i的关节配置qi满足如下式约束。
[0186]ql
≤qi≤qu[0187]
对上下界ql和qu的确定，首先为了尽可能避免发生腿与腿之间的碰撞，防止出现不稳定因素，可以适当的在不影响整体的运动性能的前提下限定1号关节的关节角度上下界。
[0188]
对于2、3号关节的关节角度上下界的确定，可以对机器人单腿的雅克比矩阵进行计算，进而由得到的雅克比矩阵计算单腿的奇异点位置，在六足机器人运动的过程中各腿
配置应尽量避免靠近奇异点。基于这样的考虑，可以得到2、3号关节的关节角度的上下界。在给定各关节角的上下界约束后，对机器人状态φ，如果逆运动学求解无实数解或得到的各腿关节配置超出了上下界，则表明该状态不符合运动学约束；否则该状态符合运动学约束。
[0189]
状态单步可达性判断算法
[0190]
时间ti到tf期间，机器人整体配置qa(ti)对应状态φ0、qa(tf)对应φ1，是否存在一单步状态转移算子使得进而能够生成一高维连续过渡轨迹使得机器人能在满足各约束的条件下从状态φ0过渡到状态φ1。状态φ0和状态φ1各数据分别记为
wrb,0
,
wrb,0
,wp
f,0
,f
f,0
,σ
f,0
,和
wrb,1
,
wrb,1
,wp
f,1
,f
f,1
,σ
f,1
,
[0191]
step1：对如下式所示的准静态稳定与运动学约束的逻辑表示判断是否为真：如果为假，则两状态不可达，退出；否则转入step2。
[0192]
[ki(φ0)＝true]∧[equil(φ0)＝true]∧[ki(φ1)＝true]∧[equil(φ1)＝true]
[0193]
step2：对无碰撞约束的逻辑表达式所示的无碰撞约束的逻辑表示判断是否为真：如果为假，则两状态不可达，退出；否则转入step3。
[0194]
[collifree(φ0)＝true]∧[collifree(φ1)＝true]
[0195]
step3：考虑到稳定性约束以及留出一定的稳定余量，两状态的公共接触点数量应大于等于3，即对逻辑判断是否为真：如果为假，则两状态不可达，退出；否则转入step4。
[0196]
s＝{i
foot
∈z:1≤i
foot
≤6,||d(i
foot
)||≤ε,σ
f,0
(i
foot
)＝σ
f,1
(i
foot
)＝1}
[0197]
|s|≥3
[0198]
step4：构建新的机器人状态φ
test0
和φ
test1
，六足机器人足端接触情况σ
f,s
∈{0,1}，是根据集合s确定的，在集合s中的编号所索引的σ
f,s
的值为1，不在该集合中的编号所索引的σ
f,s
的值为0，判断机器人状态φ
test0
和状态φ
test1
的无碰撞约束的逻辑表达是否为真：如果为假，则表示两状态不可达，退出；否则表示两状态可达，退出。
[0199][0200]
[equil(φ
test0
)＝true]∧[equil(φ
test1
)＝true]
[0201]
根据上述的4个步骤，即可判断给定的两状态的单步可达性了。值得指出的是，如果根据上述算法判别两状态单步不可达，并不意味着在实际情况中就一定不能从前一个状态转移到后一个状态——这可以分为两种情况，一种是不满足各种约束的，这种情况一定不能发生转移；另一种情况是虽然单步不可达，但是前一个状态可以通过多次单步状态转移算子的作用到达后一个状态，是多步可达的。在离散接触状态规划和路径评分应用中，利用状态单步可达性判断即可完成所有工作，且多步可达的判断可以很容易从单步可达的累积迭代拓展过去，因此不对多步可达作过多阐述。
[0202]
进一步地，本发明中，步骤五中，采用深度优先剪枝搜索算法在可达的状态序列中搜索获取存在冗余的状态的主序列的具体方法为：
[0203]
步骤五一、根据优化后的行走路径和当前状态φ
t
确定前进方向vb；
[0204][0205]
其中，r
track
为路径的跟踪系数，δrb表示一段路径的微元，xi代表路径x上第i个离散点(对应离散路径点序列中第i个元素)，r
b,t
表示t时刻机器人坐标系相对于世界坐标系的位移；
[0206]
步骤五二、根据当前状态φ
t
，确定状态转移算子中支撑摆动状态向量cf的所有取值，并通过条件1和条件2对支撑摆动状态向量cf的所有取值进行筛选，获取支撑摆动状态向量cf取值的集合sc；
[0207]
条件1：支撑摆动状态向量cf的某一位索引为1时，必须满足当前状态下各机械足与地面的物理接触状态σ
f,t
对应位索引为1，且各腿的容错状态f
f,t
对应位索引为0，
[0208]
表示当某一机械腿要作为支撑腿时必须满足其与地面物理接触且不为容错腿；
[0209]
条件2：支撑摆动状态向量cf中索引为1的数量大于等于3；表示任一状态时都至少有3条机械足同时与地面接触；
[0210]
步骤五三、基于vb和sc初步确定与每一个状态φ
t
具有状态单步可达性的下一个状态φ
t+1
的子节点集合s
φ,t
；
[0211]
步骤五四、计算集合s
φ,t
中最大前进步长l
max
；
[0212]
l
max
＝argmax(||φ:r
b-φ
t
:rb||),φ∈s
φ,t
[0213]
其中，rb为当前状态机器人世界坐标系的位移，并判断所述最大前进步长l
max
是否小于给定步长阈值ε0，若是，则停止对下一个状态的搜索，完成存在冗余的状态序列的搜索；否则，执行步骤五五；ε0的范围可以为0.2-0.5，根据不同机器人其取值范围不同；
[0214]
步骤五五、判断当前状态的所有子节点的位置与优化后的行走路径的末端位置的距离是否小于极小阈值ε1；若是，完成存在冗余的状态序列的搜索；否则，执行步骤五六；ε1的范围可以为0.1-0.2，根据不同机器人其取值范围不同；
[0215]
步骤五六、判断搜索扩展的次数是否超出极限阈值次数tmax，若是，则完成存在冗余的状态序列的搜索，否则，返回执行步骤五三，其中，tmax实际是根据电脑gpu的计算能力确定的。
[0216]
本实施方式中，确定集合s
φ,t
的流程包括以下顺序步骤，考虑到六足机器人能够全向移动，且为了尽可能突出评分用途，不考虑在路径跟踪过程中对姿态的约束，令δθb为0，δθb为地形环境特征的映射表达；
[0217]
step1：根据路径x和当前状态φ
t
确定前进方向vb[0218]
路径的跟踪系数记作rtrack，rtrack越大对路径的跟踪越粗糙，反之越精细；
[0219][0220]
step2：根据当前状态φ
t
确定状态转移算子中支撑摆动状态向量cf的可能取值，构成集合sc[0221]
当前状态下各腿与地面的物理接触状态为σf,t，各腿的容错状态为f
f,t
，则对于cf的确定有如下必须满足的条件：
[0222]
1、cf的某一位索引为1时，必须满足σ
f,t
对应位索引为1且f
f,t
对应位索引为0，表示
当某一腿要作为支撑腿时必须满足其与地面物理接触且不为容错腿。
[0223]
2、cf中索引为1的数量大于等于3，这是进行稳定转移的必要条件。基于上述条件可取的cf值可构成一集合sc。
[0224]
step3：基于vb和sc确定所有可能的与φ
t
具有状态单步可达性的下一个状态φ
t+1
，构成集合s
φ,t
对集合sc内的任一支撑摆动状态向量，以vb为六足机器人机体的前进方向，在该方向上机体的前进距离记作l；摆动腿的落足点位置为在机体前进l距离后各摆动腿满足运动学约束的任意可落足位置，对应摆动腿没有落足点则记为容错腿(对于本实施方式所提出的路径的评分需求，不考虑腿部出现驱动异常，假设所有腿都是正常可动的)，放置容错腿足端到相对于机身固定不与地面接触的位置，支撑腿的足端位置不变；基于上述描述即可得到状态转移算子进而能得到作用算子后所有可能的下一状态φ
t+1
。对所有可能的下一状态按照上一节所述的保证状态单步可达性的判断条件进行筛选，去除掉不能单步可达的状态，最终构成集合s
φ,t
。
[0225]
step4：至此，每一个状态φ
t
都能够在当前状态与给定路径的相对关系下生成一组新的状态φ
t+1
组成的集合s
φ,t
构成该状态的儿子节点，形成一个不断扩展的树形结构，这个树形结构构成了生成序列的指数增长的搜索空间。在集合s
φ,t
中应有一最大前进步长，记作lmax，其计算如下式：
[0226]
l
max
＝argmax(||φ:r
b-φ
t
:rb||),φ∈s
φ,t
[0227]
可以看到，在主序列生成算法中，当到达当前状态单步可达的所有的下一状态的步长均小于某一给定较小阈值ε0时，则认为当前状态的后续状态空间不需要继续搜索了，直接进行剪枝，回溯到父节点。
[0228]
当出现搜索的当前状态显示的位置r
b,t
与路径的末位置xn在一定极小阈值ε1下接近，或者搜索扩展的次数超出计算资源所决定的计算极限次数t
max
，则表示完成了整个主序列生成过程，进而退出搜索。
[0229]
在整个搜索过程中应该不断更新最远能够到达的序列κm，并在算法运行退出时作为最终结果。从算法的伪代码可以看到，在搜索的过程中倾向于走更大的步长并在搜索完毕较大的步长后会回退到更小的步长进行搜索。最终通过基于深度优先的剪枝搜索算法生成了一条初始粗略的由离散接触状态组成的主序列，各相邻状态满足单步可达性条件。
[0230]
进一步地，结合图5说明本实施方式，本实施方式中，步骤六中通过分治搜索算法对所述主状态序列进行优化的方法为：
[0231]
步骤六一：通过一个随机状态转移算子，考虑无碰撞和运动学等约束，生成趋向于终点的一系列单步可达的离散接触状态，形成一条随机序列；
[0232]
步骤六二：重复步骤六一，直至达到序列生成终止条件，获取a条随机序列；执行步骤六三；
[0233]
序列生成终止条件为：1、重复次数达到预设值；所述重复次数是根据计算资源确定的；2、当随机序列的状态点聚集在一个路径状态点周围，不再继续前进；3、当随机序列的所有状态点与目标状态点的距离小于距离阈值；即，生成的状态序列在一定阈值内接近目标状态序列；
[0234]
步骤六三：对所有状态节点进行状态间单步可达性判断，对单步可达的两个状态节点进行连接并标记，获取多个单步可达的状态节点；
[0235]
步骤六四：结合所述多个单步可达的状态节点，使用采用经典的贝尔曼-福特算法求解从起点状态到终点状态的最短状态序列，完成对所述主状态序列进行优化。
[0236]
基于分治搜索的主序列优化算法：
[0237]
接下来基于上述初步生成的主序列，在有限时间片内采用分治搜索的方法，进而对主序列进行进一步优化，尽可能减少状态冗余。
[0238]
先以当前状态的机体位姿为基础随机确定一倾向于向终点状态收敛的满足拓展约束的机体位姿；然后根据当前环境的可落足点情况，随机确定不与当前状态的容错向量及物理接触向量发生冲突且能够保证转移过程准静态稳定的支撑摆动状态向量；最后随机确定转移后的机体位姿下满足无碰撞约束和运动学约束的摆动腿落足点，并判断在转移后的机体位姿下支撑腿是否满足无碰撞约束和运动学约束。分治搜索算法的示意图如图三所示。
[0239]
值得指出的是，在图三中，序列连接的实线箭头也是具有状态间单步可达性的，只是为了标示单个序列的行进方向赋予其箭头。分治搜索并基于图的最短路径优化能够有效降低时间复杂度，将一个较大规模的优化问题转化为多个小规模的子问题，线性化的降低了整个问题的时间复杂度，非常有利于并行化执行，这对于快速路径评分应用来说具有非常大的优势。
[0240]
快速路径评分算法：
[0241]
基于上述两个步骤，构成整个面向交互式应用的快速路径评分算法。记运行前两节算法后得到的离散接触状态序列为κ
result
，沿用上文中提到的对于路径的表示为x。则对于路径评分的第一项可表述如式：
[0242][0243]
式中：r
bg
为序列κ
result
的最终状态所对应的机体位置。xi为待跟踪路径的第i个离散路径点。
[0244]
对于路径评分的第二项可表述如下式：
[0245][0246]
式中：κ
result
(i)—为序列κ
result
的第i个离散接触状态，n为离散点的个数；
[0247]
则对于路径评分可表示为如下式：
[0248]
ξ＝ω1ξ1+ω2ξ2[0249]
式中：ω1为路径评分第一项的权重；ω2为路径评分第二项的权重。
[0250]
以蒙特卡洛树搜索算法作为实际跟踪路径的离散接触状态规划算法，本章提出的快速评分算法对实际的跟踪效果能够作出有效预测。路径评分的第一项表示对实际跟踪路径的可通过性或完成程度进行预测评估，该项分值越大，则表明该路径将越容易被跟踪，路径的完成程度将越高。路径评分的第二项对实际跟踪路径所生成的离散接触状态序列的平均步长进行预测评估，该项分值越大，表明实际跟踪该路径将生成具有较大平均步长的高
质量状态序列。
[0251]
综上所述，可以发现在快速路径评分算法中，充斥着大量状态单步可达性判断算法的应用，同时分治搜索和随机序列的生成也对算法的并行化增加了更多可能，得益于计算机技术的飞速发展以及gpu架构的独特性，对gpu的充分利用可以极大地加速上述评分过程。
[0252]
人机协同系统框架构建
[0253]
整个软硬件架构包括人机交互系统、机载计算系统、机载运动控制系统、六足机器人硬件平台这几个部分，综合形成仿真和实物系统，如图6。各部分通过有线通信或无线通信进行连接。无线通信采用tcp/ip协议，能够保证数据的可靠传输；有线通信中，各传感器信息和电机驱动控制信息通过有线的相关协议进行有效通信。
[0254]
1)人机交互系统
[0255]
人机交互系统负责对机器人端反馈的信息进行可视化显示，以供人对机器人系统进行状态监测和行为评估，进而人通过六自由度触觉力反馈设备进行协同的运动规划以及采用各种交互方式在出现异常时快速介入、在关节层精细的对六足机器人进行调整、基于简单的基本步态对六足机器人进行直接控制等。
[0256]
六自由度触觉力反馈设备由3d systems公司研发，其具有六个输入自由度，并具有三个以力矩形式输出的三个自由度。基于此设备，如前文所述，人得以与六足机器人系统进行协同的运动规划。当机器人出现异常时，人可以通过基于mfc编写的ui界面对六足机器人进行关节层的精细控制，也可以发送急停指令保证机器人的安全，还可以发送基本的步态指令使机器人按照完全给定的周期步态序列进行无感知信息的简单移动。有时考虑到ui界面调整不方便、不自然，可以辅以语音交互的方式，定义一些基本的动作模组和元指令。多模态的信息输入方式让人更好的参与到六足机器人的运动规划过程，也促进了人机智能的融合。
[0257]
对于反馈信息来说，有多种层次和维度的反馈。具体包括：对于路径规划，会直观的将路径可视化到回传图像上，以辅助人进行路径选择；感知层的建图信息以grid_map的形式]可视化在rviz中；读取机器人定位信息和关节角信息，基于urdf将六足机器人的状态可视化在rviz中；在机器人实际执行下一步行走之前，会将规划的关节角度序列可视化，只有人经由可视化判断没有出现问题才实际执行，这样提升了冗余安全性，可以有效防止定位、感知建图、规划的某一模块出现问题时下发到执行层的命令无效导致安全风险的出现。直观、自然、信息量充分的反馈界面也更好的促进了人机智能的融合。本系统与机载计算系统由局域网连接，受益于ros的多机分布式网络通信架构，以机载计算系统作为ros的主核心，人机交互系统启动的节点能够在进行适当的配置后通过无线局域网与主核心进行通信，进而能够像在同一台计算机上一样，订阅在机载计算系统中的话题，进而对感兴趣的信息进行可视化。
[0258]
2)机载计算系统
[0259]
机载计算系统主要负责进行环境感知、机器人状态估计、路径规划、离散接触状态规划、过渡轨迹规划、人类输入信息处理等功能的计算。该系统在上下游关系中起到承上启下的作用，是整个软硬件架构中最核心的部分。该系统的硬件基础可以是常规的个人pc主机，也可以是英伟达的嵌入式主机。为了保证速度和资源利用的充分性，核心算法由c++实
现，并部署在ros(robot operating system)操作系统上。
[0260]
ros系统是一个部署在linux系统上的具有单主核心、多节点通过tcp/udp协议进行异步通信、能够进行基于服务的同步远程过程调用机制通信的机器人分布式框架。每一个节点可以看做是一个进程，每个节点内又可以开启多个线程，通过合理的软件架构设计，计算资源得以充分有效的利用。以机载计算系统为主核心，受益于ros的多机分布式网络通信架构，人机交互系统通过局域网与机载计算系统进行通信，但是在实践中发现，建立的通信是不稳定的，偶尔会出现预期功能没有顺利进行，重启所有节点才恢复的问题；这主要是由于ros的运行机制是所有节点信息交互完全通过一个主核心进行，一旦主核心由于某种原因挂掉，整个系统就会崩溃，无法按照预期功能进行。ros2从本质上解决了这个问题，即便一个节点挂掉也可以通过其他节点进行通信，提高了系统的鲁棒性，但是，考虑到ros2的开源社区相对不太完善，且对于本文研究来说并没有涉及到过多独立系统的交互，因此ros2架构的应用留作后续研究。
[0261]
接下来介绍机载计算系统中基于ros的软件架构设计。如图五是各处理进程节点与缓存数据话题的架构关系。各节点从话题中订阅数据，并基于数据运行核心程序，将处理结果发布到图中指向的话题中。规划的关节角度序列会发送到人机交互界面中进行显示，如经由人判断运行正常，则下发到实际机器人进行执行或者在gazebo中进行仿真。人机协同的路径规划模块读取人类输入信息和环境地图，不断的生成融合人类意图的待评估路径，路径评估模块动态的对待评估路径进行评估并反馈给人机交互界面供人决策，最终选定路径下发到离散接触状态规划模块。其余模块在图中可以很容易知道对应的意思，在此就不明确阐述。
[0262]
在整个软件架构的实现过程中，需要尽量提升软件系统的可扩展性、可移植性、鲁棒性以及用户友好性。可扩展性保证了在之后的系统软件开发过程中能够在兼容旧有功能的基础上快速开发新功能，在各个核心环节快速部署新的算法。这就需要充分利用c++面向对象的编程特性，充分利用封装、继承、多态的编程思想，以及创建型模式、结构型模式、行为模式等设计模式，仔细设计数据结构和各类之间的相互关系，设计出具有高可扩展性的机器人软件系统。以本系统为例，路径规划和离散接触状态规划在今后的算法研究过程中要进行经常性的版本迭代和替换，需要利用c++的多态特性，提取出所有规划算法的公有接口，以抽象基类的形式存在，在实现新的算法时只需要继承该抽象基类，实现对应的接口函数即可便捷的在整个系统应用所实现算法。同时，整个系统的架构也应该尽量模块解耦，避免出现过度耦合，导致修改扩展功能时牵一发而动全身。可移植性保证了软件系统能够在不同参数同类构型的机器人上不出错的快速部署，并对于上层智能算法来说能够将底层实物机器人的控制和仿真环境机器人的控制进行封装，进而编写出兼容异构化底层的软件系统。以本系统为例，仿真机器人和实物机器人在整个系统中的核心区别在于传感器数据的读取方法不同以及发送的角度序列控制机器人的机制不同。前者的解决方案仍旧利
[0263]
用c++的多态机制，抽象出相同接口，派生出因仿真、实物而不同的传感器数据读取类；后者的解决方案受益于ros的分布式节点架构，通过运行不同机制的控制节点即可便捷切换实物机器人及仿真机器人。在实际的算法开发过程中，考虑到实物场景的危险性以及实验场景搭建繁琐，在算法研究阶段主要在仿真环境中进行迭代，算法验证阶段在实物场景中进行。
[0264]
鲁棒性保证了程序在面对各种异常场景能够健壮运行，这一方面可以由具有高度自适应性的自主智能算法来保证，另一方面在算法的工程实现过程中也应该充分考虑到可能出现的各种边界条件，并采用高级程序语言的异常处理机制进行处理；同时也应该充分考虑到内存泄漏的问题，防止因内存泄漏造成长时间运行时上层应用程序的奔溃。以本系统为例，在工程实现过程中，大量应用了c++的trycatch机制，出现的异常得以有效及时的处理；同时，采用智能指针和引用传值，避免出现内存泄漏和大量的无用内存拷贝，提升了整个系统的运行速度和鲁棒性。
[0265]
用户友好性保证了在基于已有程序进行功能扩展和二次开发时，能够极大限度降低用户错误使用基础的工具函数的概率，以及能够使得用户在符合现有架构的基础上进行工程开发而不对现有架构造成破坏。以本系统为例，充分利用c++的封装特性，仔细设计各类的公有成员，尽可能做到对细节的有效封装和清晰明确的成员定义。
[0266]
3)机载运动控制系统
[0267]
机载运动控制系统接收上层的命令以及实际的传感信息，直接控制关节电机驱动系统，并在过程中把一些关键信息传输到机载计算系统。由于机载运动控制系统直接与底层硬件平台交互，因此无论是其硬件层面还是软件层面，可靠性和稳定性都至关重要。因此，选用倍福的工业pc作为机载运动控制系统的主机，并使用twincat软件系统，将工业pc搭载的windows系统转换为一个带有机器人实时操作系统的实时控制系统。
[0268]
实时操作系统能够在外界事件或者数据产生时快速做出响应，并在规定时间内调度一切可用计算资源完成实时任务。这非常适合于具有固定控制间隔的周期性控制任务，能够保证控制的稳定性，不至于出现间隔不定而导致跟踪发散的问题。基于实时操作系统，机载运动控制系统能够控制机器人稳定可靠的跟踪上层算法规划的关节角度序列。
[0269]
机载运动控制系统还能够直接接收从人机交互系统中输出的关节层和急停指令，能够可靠稳定的对人类的输入信息进行响应。本系统还实现了基本的周期性步态控制、机体位姿控制，在实际应用中能够帮助人进行基本的实验调试和异常处理。
[0270]
首先考虑到自主智能对复杂环境下场景的泛化性和通用性处理能力还不够强的特点，本发明通过结合人的智能，提出了一种闭环人机协同路径规划方法，该方法能够在考虑不同环境代价和兼容人类启发函数的同时生成安全、平滑的路径。针对人不擅长于进行高维空间计算的特点、本发明考虑六足机器人的准静态稳定约束、运动学约束构建了状态单步可达性判断算法，提出了一种基于深度优先剪枝搜索的主序列生成算法对路径的可跟踪性进行评估，对备选路径实际的易跟踪性及跟踪质量以路径分值的形式作出了有效预测，进而作为在线交互式的有效信息帮助人进行路径选择。本发明还通过将步态、步长、机体位姿和立足点的规划建立为一个序列优化问题，提出一种滑动蒙特卡洛树搜索得方法，该方法通过引入滑动根节点有效平衡了算法的搜索时间和采样充分性，大大提高机器人在稀疏落足点环境下的通行能力。本发明基于ros系统，设计了一个考虑人机交互的六足机器人智能规划软件框架，该系统充分利用封装、继承、多态的编程思想，以及创建型模式、结构型模式、行为模式等设计模式，仔细设计数据结构和各类之间的调用关系，设计出具有高可拓展性的规划软件框架。
[0271]
虽然在本文中参照了特定的实施方式来描述本发明，但是应该理解的是，这些实施例仅仅是本发明的原理和应用的示例。因此应该理解的是，可以对示例性的实施例进行
许多修改，并且可以设计出其他的布置，只要不偏离所附权利要求所限定的本发明的精神和范围。应该理解的是，可以通过不同于原始权利要求所描述的方式来结合不同的从属权利要求和本文中所述的特征。还可以理解的是，结合单独实施例所描述的特征可以使用在其他所述实施例中。