基于离散采样的多模态四足机器人路径规划

Path planning for multimodal quadruped robots based on discrete sampling

摘要

针对双向快速扩展随机树在多模态四足机器人路径规划中存在不必要的跳跃和行走部分路径的地形起伏程度大及转向角度变化大的问题，提出一种基于离散采样的解决方案，预处理路径去除不必要的跳跃，离散采样并动态规划获得粗解，使用B样条曲线拟合并二次规划得到最终路径。仿真结果表明：本文方法规划出的路径使机器人对质心高度的调节平均减少了31.4%，途径地形的起伏程度减小13.4%，地形倾斜角度变化降低11.4%，转向角度变化减小62.7%，证明了本文方法的有效性。

Abstract

Aiming at the challenges of unnecessary leap and significant undulations terrains with large steering angles in path planning of multimodal quadruped robots by Rapidly Exploring Random Tree algorithm， a path planning algorithm solution based on discrete sampling is proposed. The path is preprocessed to remove unnecessary leap and a solution set is obtained by discrete sampling and dynamic programming method. B-spline curves are used to define spline segments and quadratic programming method is used to optimize the final path. The simulation results show that paths planned by the proposed method exhibit an average reduction of 31.4% in the adjustment of robot's center of mass height， a 13.4% decrease in undulation of terrain， an 11.4% reduction in terrain slope angle and a 62.7% reduction in steering angle， which affirm the effectiveness of the proposed method.

Graphical abstract

关键词

人工智能 / 四足机器人 / 多模态 / 路径规划 / 离散采样 / 二次规划

Key words

artificial intelligence / quadruped robot / multimodal / path planning / discrete sampling / quadratic programming

0 引 言

四足机器人因其四肢结构的良好稳定性和足式结构的灵活性，可以在行进中实现多种运动模式切换^［1］，如行走^［2］、爬行^［3］和跳跃^［4］，在非结构化的室外复杂地形中表现出了优秀的适应能力^［5］。但运动模式的多样性和控制系统的复杂性需要搭配高效稳定的路径规划算法，现有的四足机器人路径规划通常将问题简化为二维路径规划问题，没有考虑地形起伏和倾斜程度对运动稳定性的影响，不能充分发挥多模态四足机器人的运动能力。

全局路径规划算法主要分为基于搜索的算法和基于采样的算法^［6］，基于采样的算法如快速扩展随机树^［7］（Rapidly-exploring random trees， RRT）和概率路线图算法^［8］（Probabilistic roadmap method， PRM），在复杂环境中多自由度机器人路径规划问题上的计算成本更低。高程栅格地图^［9，10］与二维栅格地图相比，带有高度和倾斜信息，能够更好地反馈地形^［11］，因此，基于采样的算法与高程地图结合是目前四足机器人路径规划问题常见的解决方案，相关学者以二者为基础，在不同的技术方向上对该问题进行了研究。Wermelinger等^［12］对高程地图处理后得到标记有可通过性的地图，使用RRT*^［13］实现全局路径规划；该方法考虑了地形信息和四足机器人的行走能力，但没有考虑使用跳跃的方式越过障碍；Norby等^［14］提出基于动力学的RRT-Connect^［15］快速规划算法（下文称“KD-RRT-Connect”）；该方法将四足机器人一次行走和跳跃组合为动作基元，通过执行一次动作基元实现搜索树节点的延伸，以此实现行走和跳跃模式相结合的规划，但该方法的行走阶段缺少对地形的综合考量，如地形的起伏和倾斜程度，并且不必要的跳跃会增加能耗和不稳定性；Wellhausen等^［16］使用概率路线图（PRM*），该方法考虑了地形可通过性和复杂地形的落足问题，充分发挥了四足机器人的行走能力，但该方法的前提是四足机器人的足端必须与地面存在接触，因此，无法对跳跃进行规划。

针对以上工作无法很好地解决多模态四足机器人使用多种运动模式组合越障和行走路线安全稳定的问题，本文基于KD-RRT-Connect算法，提出了针对多模态四足机器人的路径规划解决方案，预处理原始路径并离散采样获取周围地形信息，采用动态规划的方法获得新路线的粗解，最后通过B样条曲线拟合和二次规划进行平滑优化得到最终路径，并通过仿真验证了算法的有效性。

1 KD-RRT-Connect问题描述

基于RRT-Connect算法合成运动是一种快速高效的方法^［17］，Norby等以RRT-Connect为基础提出了适用于可跳跃的四足机器人的快速规划算法KD-RRT-Connect，该方法将一次短暂的行走和跳跃组合成动作基元，并简化使用行走和跳跃的初始加速度来表达，如式（1）所示：

\mathit{a}={\left[\begin{array}{cccc}{\mathit{a}}_{\mathrm{D}}^{\mathrm{T}}& {\mathit{a}}_{\mathrm{O}}^{\mathrm{T}}& t_{\mathrm{s}}& t_{\mathrm{f}}\end{array}\right]}^{\mathrm{T}}（1）

式中：{\mathit{a}}_{\mathrm{D}}为动作基元行走阶段的初始加速度；{\mathit{a}}_{\mathrm{O}}为动作基元跳跃阶段起跳时刻的加速度；t_s和t_f分别为行走阶段和跳跃阶段的时间。

动作基元的加速度在满足s_rand处地形摩擦约束和最大地面反馈力限制的条件下随机生成，执行动作基元的过程作为随机树的节点延伸过程，如果该过程中的行走阶段，四足机器人未出现足端全部悬空和整个过程中身体未与地形发生碰撞，就成功在随机树中添加节点。对另一棵树的启发式延伸节点过程，则使用跳跃时间为0的动作基元连接，即该过程为仅行走。初始树生成节点的过程如图1所示。

在两棵树相连后，使用仅行走的动作基元尝试在长距离连接节点，连接时仍然需要进行地形碰撞检测，如果成功使用仅行走的方式完成连接，就剔除中间的树节点，对于尝试在长距离检测中连接的两个节点，使用式（2）（3）计算该行走过程的初始和末端加速度。

{\mathit{a}}_{\mathrm{D}}=-\frac{\mathrm{6}\left({\mathit{s}}_{\mathrm{D}}-{\mathit{s}}_{\mathrm{O}}\right)+\mathrm{2}{t}_s\left(\mathrm{2}{\mathit{v}}_{\mathrm{D}}+{\mathit{v}}_{\mathrm{O}}\right)}{t_s^{\mathrm{2}}}（2）

{\mathit{a}}_{\mathrm{O}}=\frac{\mathrm{6}\left({\mathit{s}}_{\mathrm{D}}-{\mathit{s}}_{\mathrm{O}}\right)+\mathrm{2}{t}_{\mathrm{s}}\left({\mathit{v}}_{\mathrm{D}}+\mathrm{2}{\mathit{v}}_{\mathrm{O}}\right)}{t_{\mathrm{s}}^{\mathrm{2}}}（3）

式中：{\mathit{s}}_{\mathrm{D}}和{\mathit{s}}_{\mathrm{O}}分别为起始节点和终点节点的位置；{\mathit{v}}_{\mathrm{D}}和{\mathit{v}}_{\mathrm{O}}分别为起始节点和终点节点的速度；t_s为该行走阶段的持续时间。

遍历完成后再插值得到最终路径，由该过程可知，KD-RRT-Connect在延伸节点和最终插值生成路径时，仅考虑了四足机器人是否在行走时出现悬空和身体与地形发生碰撞，没有考虑实际路线的地形起伏和倾斜程度等信息，并且最终路径会存在不必要的跳跃，增加了四足机器人的能耗和完成任务的风险。

2 改进算法设计

本文改进算法设计如下：首先，对于给定KD-RRT-Connect生成的原始路径O，对其进行优化预处理，将不必要的跳跃重新规划为行走，得到路径P；其次，对路径P进行离散空间采样，从有效的采样点集合Q中获取总代价最小的粗解R；第三，对该集合进行3次B样条曲线拟合并样条插值，获取均匀点集T；最后将平滑优化问题转化为二次规划问题并使用OSQP求解器进行求解，得到优化后的最终路径U。算法流程如图2所示。

2.1　路径预处理

KD-RRT-Connect规划的路径O包含了行走和跳跃两种运动模式。由于RRT类算法自身的随机特性^［18］，路径O中的跳跃部分可能发生在没有突变的地形上，四足机器人在该段地形上可以选择更为可靠的行走方式通过，因此，本文的优化方法首先需要对路径O进行优化预处理，通过行走的方式替换不必要的跳跃动作，以增强机器人行走路线的连续性和可靠性。路径预处理的设计如下：查找原始路径O中的跳跃阶段，对起跳点和落地点之间对地形高度是否突变进行检测，当地形高度差均小于设定阈值则说明该段路径对应的地形高度无较大突变，四足机器人可以采用行走模式；对于必须使用跳跃才能通过的地形，则保留跳跃部分，然后继续完成后续路径情况的查询，遍历后得到路径P。上述过程在算法1中以伪代码表示。

算法1 路径预处理

1 输入：O从N-RRT-Connect得到的原始路径

2 输出：P路径预处理后的结果

3 P = setFlightToStance（O，0，n-1）

4 for i = 0 to n-1 do

5 if isTakeoffState（O，i） then

6 j = getLandIndex（O，i）

7 for k = i+1 to j do

8 m = getGroundHeight（O，k-1）

9 n = getGroundHeight（O，k）

10 if |m-n| > StanceLimit then

11 setStanceToFlight（P，O，i，j）

12 end

13 end

14 end

15 end

2.2　离散空间采样与动态规划

通过路径预处理操作得到的路径P，为机器人提供了一条可到达目标点的路线，相比于原始路径O去除了不必要的跳跃过程，但与原始路径O相同的是，预处理得到的路径P也没有考虑路径周围一定区域内的地形。为了在此区域内找到一条地形起伏较小和比P更平滑的路线，同时保留KD-RRT-Connect的基于概率采样的算法的优点，对P周围一定范围内的地形进行离散采样，通过采样点处的地形有效性来估计机器人的活动空间，由于限制了采样区域的大小，因此，路径P对目标点的可达性能够确保此范围内的路线变化不会改变新路径的有解性。离散采样的设计如下：从路径P的行走阶段中等间隔选取路径点P_s，对P_s中每一个路径点p_i，沿其法线方向分别在两侧等间隔选取地形上的采样点，并通过机器人是否能在该地形上有效站立这一条件来判断该点的地形有效性，最终得到地形有效的采样点q_i，将全部有效采样点的集合记为Q，此集合近似描述了机器人的活动空间。对于跳跃阶段，起跳点和落地点一定是路径P中某一行走阶段的终点或起点，因此，路径P中的跳跃过程不会影响改进算法在行走阶段的优化结果。为方便后续描述，本文假设通过预处理得到的路径P只包含行走阶段。通过对一定区域地形的离散采样来近似描述机器人活动空间的过程在算法2中以伪代码表示。

算法2 生成地形有效的采样点集合

1输入：P路径预处理后的结果

2输出：Q地形有效采样点集合

3.P_s=getRefSample（P，δd）.

4.k=getSize（P_s）

5 range=maxLateral/δ/

6 for i=0 to k do

7 heading= getHeading（P_s，i）

8 if i= 0 then

9 χ_sample= getSamplePos（P_s，heading，i，0）

10 Q ←Q ∪ ｛χ_sample｝

11 continue

12 end

13 for j= -range to range do

14 χ_sample=getSamplePos（P_s，heading，i，j）

15 if is ValidState（χ_sample） then

16 Q ←Q ∪ ｛χ_sample｝

17 end

18 end

19 end

2.2.1　离散采样代价函数

在限定活动空间后，新路线的生成过程可以通过从集合Q中的元素q_i 中选取采样点的方式表现。尽管四足机器人的足式结构可以更好地适应地形变化，但地形的倾斜程度仍然会影响机器人在行进时的稳定性，为了适应倾斜地形，四足机器人需要调整身体姿态和步态，导致更多的能量消耗。与KD-RRT-Connect相同，改进算法使用同样的模型简化方法，而细化具体落足位置和步态的调整交给底层控制而不在全局规划器中考虑。对于全局路径规划，机器人仍需要避免过于倾斜和复杂的地形，以协调整体任务难度，但只选择平坦路线的策略相当于舍弃了足式结构对地形的灵活适应性，因此，优化路线的选择需要同时考虑地形倾斜的变化和机器人姿态的变化。

地形倾斜程度可通过倾斜角度来描述。对采样点集合Q中每一个地形有效的采样点q_（i，j），可以使用式（4）计算该处位置地形的倾斜角度。

{\theta }_{\left(i,j\right)}=\mathrm{a}\mathrm{r}\mathrm{c}\mathrm{c}\mathrm{o}\mathrm{s}\left(\frac{{\mathit{n}}_{q_{\left(i,j\right)}}\cdot \mathit{z}}{\left|{\mathit{n}}_{q_{\left(i,j\right)}}\right|\cdot \left|\mathit{z}\right|}\right)（4）

式中：θ_（i，j） 为采样点q_（i，j） 处地形的倾斜角度； n 为对应采样点的地形单位法向量，可以通过所处地图网格的梯度分量计算得到；\mathit{z}为世界坐标系下 z 方向的单位向量。

机器人的姿态变化可以通过两个采样点之间的距离变化、高度变化和机器人在采样点位置的转向变化近似描述。改进算法将四足机器人在两个采样点上的状态转移过程以代价函数的形式表示，状态转移包含机器人的姿态变化和采样点的地形倾斜变化，机器人在某一采样点的代价递推定义如下：

Cost=Cos{t}_{\mathrm{t}\mathrm{r}\mathrm{a}\mathrm{n}\mathrm{s}}+Cos{t}_{\mathrm{p}\mathrm{r}\mathrm{e}}（5）

\begin{array}{l}Cos{t}_{\mathrm{t}\mathrm{r}\mathrm{a}\mathrm{n}\mathrm{s}}=Cos{t}_{\mathrm{d}\mathrm{i}\mathrm{s}}+Cos{t}_{\mathrm{d}\mathit{z}}+Cos{t}_{\mathrm{a}\mathrm{n}\mathrm{g}}+\\ Cos{t}_{\mathrm{s}\mathrm{l}\mathrm{o}\mathrm{p}\mathrm{e}}+Cos{t}_{\mathrm{g}\mathrm{o}\mathrm{a}\mathrm{l}}\end{array}（6）

式中：Cost_trans为机器人的状态转移代价；Cost_pre为前一采样点的代价，起点处的代价为0；Cost_dis、Cost_dz 、Cost_ang、Cost_slope和Cost_goal 分别为距离代价、地形高度变化代价、转向变化代价、地形倾斜变化产生的代价和终点位置代价。

对于每一项代价，可以通过以下公式计算得到：

Cos{t}_{\mathrm{d}\mathrm{i}\mathrm{s}}=w_{\mathrm{d}\mathrm{i}\mathrm{s}}\delta d（7）

Cos{t}_{\mathrm{d}\mathit{z}}=w_{\mathrm{d}\mathit{z}}\delta z（8）

Cos{t}_{\mathrm{a}\mathrm{n}\mathrm{g}}=w_{\mathrm{p}\mathrm{r}\mathrm{e}}\mathrm{\Delta }{\alpha }_{\mathrm{p}\mathrm{r}\mathrm{e}}+w_{\mathrm{p}}\mathrm{\Delta }{\alpha }_{\mathrm{p}}（9）

Cos{t}_{\mathrm{s}\mathrm{l}\mathrm{o}\mathrm{p}\mathrm{e}}=w_{\mathrm{s}\mathrm{l}\mathrm{o}\mathrm{p}\mathrm{e}}\mathrm{\Delta }\theta（10）

Cos{t}_{\mathrm{g}\mathrm{o}\mathrm{a}\mathrm{l}}=w_{\mathrm{g}\mathrm{o}\mathrm{a}\mathrm{l}}\mathrm{\Delta }l/\delta n（11）

式中：w_dis为距离代价权重；w_dz 为高度变化权重；w_pre为前采样点的转向变化权重，w_p为当前采样点转向变化权重；w_slope为地形倾斜变化权重；w_goal为终点位置权重；δd为采样点的欧氏距离；δz为采样点在地形上的高度差；Δα_pre和Δα_p分别为采样点连线方向与前采样点和当前采样点方向的变化角度；Δθ为采样点地形倾斜角度的变化；Δl表示采样点与终点的横向索引差值；δn为当前采样点纵向索引与终点索引的差值。

由式（5）~式（11）可知，通过设置递推代价函数，将查找最优路线的过程转化为机器人在起点和终点之间的离散空间搜索粗解的过程，该过程会综合考虑距离、地形高度、转向角度、地形倾斜程度和与终点位置关系的变化。

2.2.2　动态规划

采样点集合Q中存在多个不同的解，机器人通过已设置的代价函数从其中找到代价最小的最优路线。从采样点集合Q中查找粗解R的过程是一个经典的动态规划问题。动态规划是求解决策过程最优化的一种方法，通过将待求解问题拆分为多个相同的子问题，分别对子问题求解并保存已解决子问题的结果，从而提高算法效率，避免了大量重复计算。该过程在算法3中以伪代码表示。

算法3 动态规划获得粗解

1 输入：O地形有效的采样点集合2输出：R最优路线的粗解

3 minCost←∞

4 for each q_i ∈Q do

5 for each q_（i，j）∈q_i do

6 for each q_{（i-1，k）}∈q_i-1 do

7 Cost= calTotalCost（q_{（i-1，k），}q_（i，j））

8 if Cost < minCost then

9 minCost= Cost

10 q_{（i-1，k）}←Parent（q_（i，j））

11 end

12 end

13 if hasParent（q_（i，j）） then

14 setCost（q_（i，j）），minCost）

15 end

16 end

17 end

18 r= getEndPoint（q_i-1）

19 R←getPreToBegin（r）

基于设置的代价函数，机器人通过动态规划搜索到了一条更适合的路线，解决了机器人在地形选择上产生的决策问题。

2.3　三次B样条曲线拟合

粗解R是从集合Q中选择的离散点，其离散性受初始采样间隔的制约，并且存在疏密不均的情况，这种离散的粗解无法直接用于机器人导航，因此，要对粗解R进一步处理，得到连续的曲线，在路径规划问题中通常使用曲线拟合的方式将离散点处理成光滑的曲线。B样条曲线具有局部性、连续性和凸包性的优点，非常适合将离散点拟合成曲线。k阶B样条曲线定义如式（12）所示：

P\left(u\right)={\displaystyle \sum _{i=\mathrm{0}}^n}{P}_i{B}_{i,k}\left(u\right)（12）

式中：P_i 为控制点；B_i，k （u）为基函数，基函数参考deBoor-Cox递推定义^［19］。

本文使用三次B样条曲线。粗解R中的点即给定控制点，使用Steinbeck的Tinyspline曲线库^［20］得到B样条曲线的离散点，记该离散点集合为S。但曲线拟合无法完全解决原本粗解R中点的稀疏程度不同的问题，为了曲线上的点均匀分布，需要对集合S进行3次样条插值，通过固定的间隔对S中的点进行插值处理，得到均匀分布的离散点集合T。

2.4　基于二次规划的平滑优化

曲线拟合和样条插值目的是实现离散点拟合并均匀分布，拟合对粗解R的平滑优化主要体现在局部范围，不能替代整体路径的平滑处理过程。在全局路径规划中，整体路径的平滑优化可以转化为二次规划问题并使用求解器解决。当目标函数为二次型并且约束条件是线性时，可以将求函数最小值的问题转化为二次规划问题。二次规划问题可以用式（13）（14）的形式描述：

\mathrm{m}\mathrm{i}\mathrm{n}\left(\frac{\mathrm{1}}{\mathrm{2}}{\mathit{x}}^{\mathrm{T}}\mathit{H}\mathit{x}+{\mathit{g}}^{\mathrm{T}}\mathit{x}\right)（13）

\mathit{l}\le \mathit{A}\mathit{x}\le \mathit{u}（14）

式中： x 为待求解的变量； H 为hessian矩阵； g 为梯度矩阵； A 为约束矩阵； l 和 u 分别为约束条件的下界和上界。

四足机器人作为一种地面机器人，路径的平滑过程主要表现在行走时的偏航角变化，因此，优化变量为离散点t_i （x_i， y_i ）。全局路径规划平滑优化的代价函数可以用式（15）表示。

Cost=Cos{t}_{\mathrm{s}\mathrm{m}\mathrm{o}\mathrm{o}\mathrm{t}\mathrm{h}}+Cos{t}_{\mathrm{l}\mathrm{e}\mathrm{n}\mathrm{g}\mathrm{t}\mathrm{h}}+Cos{t}_{\mathrm{d}\mathrm{e}\mathrm{v}}（15）

式中：Cost_smooth为平滑度；Cost_length为长度；Cost_dev为相对参考点的偏移量。

路径的平滑程度可以通过式（16）使用相邻点的位置关系表示。

\begin{array}{c}Cos{t}_{\mathrm{s}\mathrm{m}\mathrm{o}\mathrm{o}\mathrm{t}\mathrm{h}}=w_{\mathrm{s}\mathrm{m}\mathrm{o}\mathrm{o}\mathrm{t}\mathrm{h}}\\ {\displaystyle \sum _{i=\mathrm{1}}^{n-\mathrm{2}}}\left({\left(x_{i-\mathrm{1}}+x_{i+\mathrm{1}}-\mathrm{2}{x}_i\right)}^{\mathrm{2}}+{\left(y_{i-\mathrm{1}}+y_{i+\mathrm{1}}-\mathrm{2}{y}_i\right)}^{\mathrm{2}}\right)\end{array}（16）

式中：w_smooth为平滑度的权重系数。

长度的优化目标是使优化后的点尽可能紧凑分布，代价定义如式（17）所示：

Cos{t}_{\mathrm{l}\mathrm{e}\mathrm{n}\mathrm{g}\mathrm{t}\mathrm{h}}=w_{\mathrm{l}\mathrm{e}\mathrm{n}\mathrm{g}\mathrm{t}\mathrm{h}}{\displaystyle \sum _{i=\mathrm{1}}^{n-\mathrm{1}}}\left({\left(x_i-x_{i-\mathrm{1}}\right)}^{\mathrm{2}}+{\left(y_i-y_{i-\mathrm{1}}\right)}^{\mathrm{2}}\right)（17）

式中：w_length为长度的权重系数。

如果相对参考点的偏移量过大，则会丢失原有的几何信息，并且与样条插值后的点集产生较大的偏离可能导致机器人与地形发生碰撞。对偏移量的代价定义如式（18）所示：

Cos{t}_{\mathrm{d}\mathrm{e}\mathrm{v}}=w_{\mathrm{d}\mathrm{e}\mathrm{v}}{\displaystyle \sum _{i=\mathrm{0}}^{n-\mathrm{1}}}\left({\left(x_i-x_{\mathrm{r}\mathrm{e}{\mathrm{f}}_i}\right)}^{\mathrm{2}}+{\left(y_i-y_{\mathrm{r}\mathrm{e}{\mathrm{f}}_i}\right)}^{\mathrm{2}}\right)（18）

式中：w_dev为相对偏移量的权重系数。

相对参考点的偏移量的边界条件通过式（19）（20）表示。

x_{\mathrm{r}\mathrm{e}\mathrm{f}}-x_l\le x\le x_{\mathrm{r}\mathrm{e}\mathrm{f}}+x_u（19）

y_{\mathrm{r}\mathrm{e}\mathrm{f}}-y_l\le y\le y_{\mathrm{r}\mathrm{e}\mathrm{f}}+y_u（20）

曲率约束是非线性约束，并且与机器人的自身性能有关，不在全局路径规划中考虑。待优化的目标函数能够写成标准的二次型形式，约束条件为线性，本文使用OSQP求解器^［21］对其进行求解，得到优化后的路径点集合T_{\mathrm{s}}。该集合包含了新路线的二维坐标，对于机器人身体的竖直方向的维度，本文选择与KD-RRT-Connect中相似的处理方法，但尽可能使机器人对自身质心高度的调节较小并限定在一定范围内。由于地形高度与位置的关系由地形环境本身决定，无法通过等式表达，因此，对该维度单独处理，具体做法是先通过集合T_s计算在机器人质心相对地面的高度不变时，质心在世界坐标系下的高度。该高度等于某一处地形表面的高度与机器人初始质心高度的和，如式（21）所示：

z_{\mathrm{r}\mathrm{e}\mathrm{f}}=H\left(x,y\right)+H_{\mathrm{n}\mathrm{o}\mathrm{r}\mathrm{m}\mathrm{a}\mathrm{l}}（21）

式中：H（x， y）为（x， y）位置的地形高度；H_normal为初始状态机器人质心距离地面的高度。

优化目标是机器人自身对质心高度变化的调节，使用式（22）定义。

Cos{t}_z={\displaystyle \sum _{i=\mathrm{1}}^{n-\mathrm{1}}}(z_i-z_{i-\mathrm{1}}{)}^{\mathrm{2}}（22）

相对参考点的偏移量用式（23）表示。

z_{\mathrm{r}\mathrm{e}\mathrm{f}}-z_l\le z\le z_{\mathrm{r}\mathrm{e}\mathrm{f}}+z_u（23）

优化目标函数为二次型形式，约束条件为线性，同样使用OSQP求解，并将求解后的结果与T_s组合成三维坐标，得到最终路径U。

3 实验与分析

为验证改进算法的有效性，本文基于ROS noetic版本，在相同地图、相同起点和终点的初始条件下，使用KD-RRT-Connect和本文的改进算法进行仿真对比实验。仿真的初始地图如图3所示。

地图的环境参数如表1所示。由于RRT类算法具有随机性，本文为了保证规划结果的准确性和稳定性，对两种算法各运行100次。

同时，为了验证改进算法在不同复杂程度地形上的效果，本文对初始地图的网格在高度上分别添加0~5 cm和0~10 cm的随机量，并分别在图4（b）（c）两个更复杂的地图上运行两种算法各100次。

3.1　仿真结果分析

图5是两种算法在不同复杂程度地形中的部分运行结果。其中，黑色曲线代表原始算法KD-RRT-Connect，白色曲线代表本文的改进算法。在（a-1）中，原始路径在一处凸起附近有较大的转向角度，而在改进算法中，路径绕过凸起并且转弯时的曲率较小，提高了机器人在行走阶段的稳定性。在（a-3）（b-1）（c-1）和（c-3）中，原始路径在经过KD-RRT-Connect算法长距离检测并剔除冗余路径点后仍然存在不平滑的部分，说明该处不平滑区域是不必要的跳跃模式导致，改进算法在判断地形没有较大突变后，采用行走的运动模式替换原有不必要的跳跃并最终优化为更平滑的路径，减小了多模态四足机器人在此部分的能耗并提高了执行任务的稳定性。其余结果则表现了改进算法对路径的平滑优化和考虑了地形信息的新路线的选择，如（a-2）和（b-2），改进算法生成的路径选择沿浅坑的边缘行进而非进入浅坑，减小了地形起伏程度的变化并缩短了路径长度。整体来看，改进算法生成的路径表现更平滑，转向角度更小，地形选择更平坦，提高了多模态机器人行进的稳定性。

3.2　计算结果分析

本文以路径长度、机器人质心高度变化、地形起伏高度变化、地形倾斜变化和转向角度变化为评价指标，分别计算了两种算法在不同复杂程度的地形下运行结果的平均值和方差，如表2和表3所示。

图6展示了两种算法在路径长度和地形起伏高度变化上的分布情况，从散点图中可以看出，尽管RRT类算法具有随机特性，散点分布不会特别集中，但从分布趋势可以看出，改进算法在这两个指标上都优于原始算法，平均路径长度更短，途径地形的高度变化也更小，有利于降低多模态四足机器人的能量消耗和提高行进的稳定性。

图7（a）（b）分别展示了两种算法在质心高度变化和地形起伏变化、地形倾斜变化和转向角度变化的对比结果，O代表原始算法，U代表改进算法。改进算法相比原始算法在平均路径长度上缩短了5.8%，在质心变化这一指标上缩短了31.4%，显著减少了机器人在行进时对自身质心高度的调整，更节省能量。在地形方面，改进算法的地形起伏和地形倾斜相比原始算法分别降低了13.4%和11.4%，能减少机器人在有起伏的地形上行走，并且地形倾斜变化也有所降低，提高了四足机器人行走时的安全性和稳定性。在转向变化上，改进算法相比原始算法降低了62.7%，显著改善了KD-RRT-Connect在规划时出现的路径转向次数较多和部分转向角度大的情况，改进算法的路径更平滑。方差结果显示，改进算法指标的方差除了倾斜程度略大于原始算法外，其余指标的方差均小于原始算法，说明在绝大多数情况下，改进算法的浮动较小，规划结果更稳定，保证了机器人行进时的稳定性。

4 结束语

本文针对目前多模态四足机器人在路径规划中存在不必要的跳跃和行走部分路径质量变化大的问题，提出了一种基于离散采样的解决方案，通过优化原始路径中多模态四足机器人的运动策略，离散采样获取周围地形信息，最终得到平滑优化的新路径。仿真结果表明：相比于原始算法，改进算法能缩短路径长度，减少地形起伏变化和地形倾斜程度变化，降低机器人自身对质心高度的调节和平滑路径的急转部分，减小了总体转向角度，并且规划的结果浮动更小。本文方法能够优化多模态四足机器人的运动策略从而降低能耗，考虑地形信息并提高行进路线的稳定性，为具有多种运动模式的多模态机器人在实际环境中的全局路径规划奠定基础。

参考文献

[1] 张秀丽, 王琪, 黄森威, 等. 一种多模型融合的仿猎豹四足机器人复杂运动控制方法[J]. 机器人, 2022, 44(6): 682-693.

[2] Zhang Xiu-li, Wang qi, Huang Sen-wei, et al. A multi-model fusion based complex motion control approach for a cheetah-mimicking quadruped robot[J]. Robot, 2022, 44(6): 682-693.

[3] 张帅帅, 荣学文, 李贻斌, 等. 崎岖地形环境下四足机器人的静步态规划方法[J]. 吉林大学学报: 工学版, 2016, 46(4): 1287-1296.

[4] Zhang Shuai-shuai, Rong Xue-wen, Li Yi-bin, et al. Static gait planning method for quadruped robots on rough terrains[J]. Journal of Jilin University (Engineering and Technology Edition), 2016, 46(4): 1287-1296.

[5] 周坤, 李川, 李超, 等. 面向未知复杂地形的四足机器人运动规划方法[J]. 机械工程学报, 2020, 56(2): 210-219.

[6] Zhou Kun, Li Chuan, Li Chao, et al. Motion planning method for quadruped robots walking on unknown rough terrain[J]. Journal of Mechanical Engineering, 2020, 56(2): 210-219.

[7] Nguyen Q, Powell M J, Katz B, et al. Optimized jump on the mit cheetah 3 robot[C]∥International Conference on Robotics and Automation, Montreal, Canada, 2019: 7448-7454.

[8] Jenelten F, He J, Farshidian F, et al. DTC: deep tracking control[J]. Science Robotics, 2024, 9(86): eadh5401.

[9] 崔炜, 朱发证. 机器人导航的路径规划算法研究综述[J]. 计算机工程与应用, 2023, 59(19): 10-20.

[10] Cui Wei, Zhu Fa-zheng. Review of path planning algorithms for robot navigation[J]. Computer Engineering and Applications, 2023, 59(19): 10-20.

[11] Lavalle S. Rapidly-exploring random trees: a new tool for path planning[J]. The Annual Research Report, 1998, 6: 25989663.

[12] Kavraki L E, Svestka P, Latombe J C, et al. Probabilistic roadmaps for path planning in high-dimensional configuration spaces[J]. IEEE transactions on Robotics and Automation, 1996, 12(4): 566-580.

[13] Fankhauser P, Hutter M. A universal grid map library: Implementation and use case for rough terrain navigation[J]. Robot Operating System (ROS) The Complete Reference, 2016, 1: 99-120.

[14] Fankhauser P, Bloesch M, Gehring C, et al. Robot-centric elevation map with uncertainty estimates[C]∥ 17th International Conference on Climbing and Walking Robots and the Support Technologies for Mobile Machines, Poznan, Poland, 2014: 433-440.

[15] 张慧, 荣学文, 李贻斌, 等. 四足机器人地形识别与路径规划算法[J]. 机器人, 2015, 37(5): 546-556.

[16] Zhang hui, Rong Xue-wen, Li Yi-bin, et al. Terrain recognition and path planning for quadruped robot[J]. Robot, 2015, 37(5): 546-556.

[17] Wermelinger M, Fankhauser P, Diethelm R, et al. Navigation planning for legged robots in challenging terrain[C]∥IEEE/RSJ International Conference on Intelligent Robots and Systems, Daejeon, South Korea, 2016: 1184-1189.

[18] Karaman S, Frazzoli E. Sampling-based algorithms for optimal motion planning[J]. The International journal of Robotics Research, 2011, 30(7): 846-894.

[19] Norby J, Johnson A M. Fast global motion planning for dynamic legged robots[C]∥IEEE/RSJ International Conference on Intelligent Robots and System, Las Vegas, USA, 2020: 3829-3836.

[20] Kuffner J J, LaValle S M. RRT-connect: an efficient approach to single-query path planning[J]. IEEE International Conference on Robotics and Automation. Symposia Proceedings, 2000, 2: 995-1001.

[21] Wellhausen L, Hutter M. Rough terrain navigation for legged robots using reachability planning and template learning[C]∥IEEE/RSJ International Conference on Intelligent Robots and Systems, Vancouver, Canada, 2021: 6914-6921.

[22] Fernbach P, Tonneau S, Del Prete A, et al. A kinodynamic steering-method for legged multi-contact locomotion[C]∥IEEE/RSJ International Conference on Intelligent Robots and Systems, Vancouver, Canada, 2017: 3701-3707.

[23] 吴振宇, 刘小飞, 王义普. 基于DKRRT*-APF算法的无人系统轨迹规划[J]. 吉林大学学报: 工学版, 2023, 53(3): 781-791.

[24] Wu Zhen-yu, Liu Xiao-fei, Wang Yi-pu. Trajectory planning of unmanned system based on DKRRT*⁃APF algorithm[J]. Journal of Jilin University (Engineering and Technology Edition), 2023, 53(3): 781-791.

[25] Deboor C. On calculating with B-splines[J]. Journal of Approximation Theory, 1972, 6(1): 50-62.

[26] Steinbeck M, Koschke R. Tinyspline: a small, yet powerful library for interpolating, transforming, and querying nurbs, b-splines, and bézier curves[C]∥IEEE International Conference on Software Analysis, Evolution and Reengineering, Honolulu, USA, 2021: 572-576.

[27] Stellato B, Banjac G, Goulart P, et al. OSQP: an operator splitting solver for quadratic programs[J]. Mathematical Programming Computation, 2020, 12(4): 637-672.

基金资助

国家自然科学基金项目(52105081)

国家自然科学基金项目(52005474)

国家自然科学基金项目(62303002)

国家自然科学基金优秀青年科学基金项目（海外）(GG2090007004)

中国科学技术大学启动基金项目(KY2090000067)