复杂地形环境下无人步履式挖掘机的车身姿态规划方法

Body Posture Planning Method for Unmanned Walking Excavators under Complex Terrain Environments

摘要

非结构化地形作业中，无人步履式挖掘机的姿态规划对底盘稳定性与作业安全起着至关重要的作用。针对车身姿态控制问题，提出一种基于多目标优化的姿态规划方法。建立底盘的七自由度运动学模型，并利用NURBS曲线生成步行腿轨迹。以姿态偏差和能耗为优化目标，构建融合地形约束的非线性多目标规划模型。数学-物理联合仿真及实车实验表明，所提方法可保证无人步履式挖掘机以良好的姿态穿越复杂地形。

Abstract

In unstructured terrain operations， the posture planning of UWE was of great significance for ensuring chassis stability and operational safety. A multi-objective optimization-based posture planning method was proposed herein. A 7-DOF kinematic model of the chassis was established， and the walking leg trajectories were generated by NURBS curves. A nonlinear multi-objective planning model integrating terrain constraints was constructed， with posture deviation and energy consumption were defined as optimization objectives. The proposed method was validated through high-fidelity mathematical-physical co-simulation and field experiments. The proposed planning method ensures that the UWE may traverse complex terrain with good posture.

Graphical abstract

关键词

无人步履式挖掘机 / 多目标优化 / 非均匀有理B样条曲线 / 非线性轨迹规划

Key words

unmanned walking excavator（UWE） / multi-objective optimization / NURBS curve / nonlinear trajectory planning

0 引言

步履式挖掘机作为一种全地形挖掘机，常用于非结构化地形的应急救援和军事行动^［1-6］。复杂的地形条件和机械结构使得步履式挖掘机操作难度大、工作效率低、能耗高，甚至存在翻车等作业风险^［7-13］。为应对上述问题，许多研究者对步履式挖掘机的自动化、无人化作业进行了研究。无人步履式挖掘机（unmanned walking excavator， UWE）的各种自动化技术中，底盘稳定性控制是UWE的开发基础，直接影响UWE的操作稳定性和作业安全性。

步履式挖掘机的自动化研究主要分为两类：

1）将多足机器人的力控方法转移到步履式挖掘机的控制上^［14-17］。RAIBERT等^［18］利用关节传感器信息确定机器人与地面接触的时刻，计算每条腿和执行器所需的负载，通过姿态控制算法控制机器人的位姿。HUTTER等^［19］采用基于模型的控制策略，通过控制扭矩来调控机器人的运动，使四足机器人能在复杂地形上完成从步行到奔跑的各种步态。HUTTER等^［20-21］使用一种带有压力反馈的新型液压阀实现液压缸的力跟踪，并使用虚拟模型控制优化四条步行腿地面接触力的分配。上述方法大多只考虑对机器人足部接触力或关节扭矩的控制，忽略了对底盘姿态的调节，可能导致UWE行驶过程中出现较大颠簸，甚至发生倾翻。

2）采用轨迹规划的方法规划UWE的运动^［22-24］。BJELONIC等^［25］提出一种针对轮腿式机器人的实时轨迹优化方法，将整体运动规划简化为车轮和底座的轨迹规划，通过模型预测控制策略进行求解。SUN等^［26］开发了多足机器人在不同运动模式下的足部位置规划方法，通过引入包含足部速度信息的步态图来提高混合运动规划的连续性与协调性。 JELAVIC等^［27-29］使用基于分层优化的全身控制器实现了对步履式挖掘机整车运动的组合规划和轨迹跟踪。步履式挖掘机运动规划虽然已有研究，但大多数研究都没有考虑复杂地形对UWE的影响。

上述运动规划方法的研究未充分考虑复杂地形对运动学特性的影响，现有优化模型普遍忽略运动过程中的车身平顺性与能耗问题，因此难以获得能量消耗最小的步行腿运动轨迹。复杂地形下，为保持车身稳定性，UWE需对4条步行腿进行独立控制，而各步行腿的运动轨迹通常具有明显的非线性与不规则性。常见的轨迹描述方法，如多元多项式法和B样条法，受曲线自由度限制，难以精确表征步行腿的复杂运动特征。

针对上述问题，本文提出UWE的总体开发框架，并从提高自主作业安全性出发，设计了一种基于多目标优化的车身稳定性控制方法。为研究车身姿态与车辆前方地形之间的耦合关系并提高实时轨迹规划性能，基于闭环矢量方程推导了底盘七自由度车身-地形耦合运动学模型，采用NURBS曲线构造步行腿升降液压缸的运动轨迹，提出NURBS曲线插值的离散点连续时间重构方法来准确描述4条步行腿的不规则运动轨迹。在此基础上构建融合多目标、多约束及地形信息的非线性轨迹规划模型以最小化UWE作业过程中的底盘姿态偏差和能耗。最后，采用数学-物理联合仿真与实车实验验证该方法的性能。

1 UWE框架组成

UWE主要由轮腿式底盘和工作装置组成。回转平台以上结构与无尾型液压挖掘机相近，底盘四角安装有4条可做多自由度运动的铰接式步行腿。工作装置为全回转、短动臂、伸缩斗杆式反铲结构，并且可换接各种液压机具。

为实现底盘姿态的自主决策，本文基于传统的步履式挖掘机结构开发了一种智能底盘控制方法。如图1所示，驾驶室上方放置了感知环境的激光雷达（LiDAR）以获取车辆前方的地形信息；全球导航卫星系统（global navigation satellite system， GNSS）和惯性测量单元（inertial measurement unit， IMU）确定UWE位置和底盘姿态；步行腿液压缸集成磁致伸缩位移传感器，实现对活塞杆行程的实时检测。将上述传感器采集的数据输入工控机，通过对UWE的整体非线性轨迹进行规划，并将规划结果转化为步行腿升降液压缸的控制指令，以实现对底盘姿态的调节。所提方法通过合理规划步行腿在复杂地形上的运动，确保了底盘的稳定性。

2 UWE系统建模

为在各种复杂地形下实现底盘姿态的实时控制，结合车辆前方地形数据，基于闭环矢量方程推导了UWE的七自由度非线性车身-地形耦合运动学模型。

2.1 坐标系定义

如图2所示，参考坐标系｛U｝固结于大地，不随UWE位置的变化而改变，主要用来描述UWE的空间位置和方向，其原点O_U 与底盘初始位置的重心重合；X_U 轴的方向与行驶方向相同；Z_U 轴的方向与重力方向相反；Y_U 轴垂直于X_UO_UZ_U 平面且方向符合右手定则。为描述UWE的运动状态，引入局部坐标系｛A｝。坐标系｛A｝是固结于车身且随车身位置变化而改变的随体坐标系，原点O_A 固结于底盘重心处，X_A 轴指向车辆前方，Y_A 轴指向车辆左侧，Z_A 轴指向车辆上方。定义E、F、G、H分别为UWE左前、右前、左后、右后的底盘与4条步行腿的铰接点，I、J、K、L分别为相应的步行腿与轮胎的铰接点。

点P从局部坐标系｛A｝到参考坐标系｛U｝的坐标变换关系可表示为^［30］

\left[\frac{{}_{}{}^U\mathit{P}}{\mathrm{1}}\right]=\left[\begin{array}{cccc}& {}_A^U\mathit{R}& & {}_{}{}^U{\mathit{P}}_{AO}\\ \mathrm{0}& \mathrm{0}& \mathrm{0}& \mathrm{1}\end{array}\right]\left[\frac{{}_{}{}^A\mathit{P}}{\mathrm{1}}\right]（1）

{}_A^U\mathit{R}=\left[\begin{array}{ccc}\mathrm{c}\mathrm{o}\mathrm{s}{\theta }_{\mathrm{s}}& \mathrm{s}\mathrm{i}\mathrm{n}{\theta }_{\mathrm{s}}\mathrm{s}\mathrm{i}\mathrm{n}{\phi }_{\mathrm{s}}& \mathrm{c}\mathrm{o}\mathrm{s}{\phi }_{\mathrm{s}}\mathrm{s}\mathrm{i}\mathrm{n}{\theta }_{\mathrm{s}}\\ \mathrm{0}& \mathrm{c}\mathrm{o}\mathrm{s}{\phi }_{\mathrm{s}}& -\mathrm{s}\mathrm{i}\mathrm{n}{\phi }_{\mathrm{s}}\\ -\mathrm{s}\mathrm{i}\mathrm{n}{\theta }_{\mathrm{s}}& \mathrm{c}\mathrm{o}\mathrm{s}{\theta }_{\mathrm{s}}\mathrm{s}\mathrm{i}\mathrm{n}{\phi }_{\mathrm{s}}& \mathrm{c}\mathrm{o}\mathrm{s}{\theta }_{\mathrm{s}}\mathrm{c}\mathrm{o}\mathrm{s}{\phi }_{\mathrm{s}}\end{array}\right]（2）

式中： ^UP_AO 为点O_A 相对于点O_U 的位置矢量；{}_A^U\mathit{R}为坐标系｛A｝相对于｛U｝的旋转矩阵；θ_s、φ_s分别为底盘的俯仰角与侧倾角。

则式（1）可改写为矩阵形式：

{}_{}{}^U\mathit{P}{=}_A^U\mathit{T}{}_{}{}^A\mathit{P}（3）

式中： ^UP 、 ^AP 分别为点P在参考坐标系｛U｝与局部坐标系｛A｝下的齐次坐标；{}_A^U\mathit{T}为从坐标系｛A｝到｛U｝的齐次变换矩阵。

2.2 运动学建模

如图3所示，以左前腿为例推导考虑路面激励的七自由度UWE非线性车身-地形耦合运动学模型，基于闭环矢量法，可建立如下方程：

\mathit{r}={\mathit{r}}_{\mathrm{1}}+{\mathit{r}}_{\mathrm{2}}+{\mathit{r}}_{\mathrm{3}}+{\mathit{r}}_{\mathrm{4}}=\mathit{O}\mathit{o}+\mathit{o}\mathit{E}+\mathit{E}\mathit{I}+{\mathit{r}}_{\mathrm{4}}（4）

式中： r₁为底盘重心处的位移矢量； r₂为底盘重心指向底盘铰接点的矢量； r₃为底盘铰接点指向轮胎铰接点的矢量； r₄为轮胎铰接点指向轮胎接地点的矢量。

为描述点I的坐标，在点E处引入额外的局部坐标系｛e｝。参考系｛e｝基于局部坐标系｛A｝，并通过平移与旋转变换得到，其原点在点E，X_A 轴方向与目标步行腿的轴线方向一致。由式（1）～（3）可求得点E和点I在参考坐标系｛U｝中的位置矢量：

{}_{}{}^U\mathit{E}{=}_A^U\mathit{T}{}_{}{}^A\mathit{E}{=}_A^U\mathit{T}{[\begin{array}{cccc}{l}_{\mathrm{a}}& l_{\mathrm{c}}/\mathrm{2}& \mathrm{0}& \mathrm{1}\end{array}]}^{\mathrm{T}}（5）

{}_{}{}^U\mathit{I}={}_A{}^U\mathit{T}{}_e{}^A\mathit{T}{}_{}{}^e\mathit{I}={}_A{}^U\mathit{T}\left[\begin{array}{cccc}\begin{array}{c}\begin{array}{c}\mathrm{c}\mathrm{o}\mathrm{s}{\theta }_{\mathrm{L}\mathrm{F}}\\ \mathrm{0}\end{array}\\ -\mathrm{s}\mathrm{i}\mathrm{n}{\theta }_{\mathrm{L}\mathrm{F}}\end{array}& \begin{array}{c}\mathrm{0}\\ \mathrm{1}\\ \mathrm{0}\end{array}& \begin{array}{c}\mathrm{s}\mathrm{i}\mathrm{n}{\theta }_{\mathrm{L}\mathrm{F}}\\ \mathrm{0}\\ \mathrm{c}\mathrm{o}\mathrm{s}{\theta }_{\mathrm{L}\mathrm{F}}\end{array}& \begin{array}{c}{l}_{\mathrm{a}}\\ l_{\mathrm{c}}/\mathrm{2}\\ \mathrm{0}\end{array}\\ \mathrm{0}& \mathrm{0}& \mathrm{0}& \mathrm{1}\end{array}\right]\left[\begin{array}{c}{l}_{\mathrm{l}\mathrm{e}\mathrm{g}}\\ \mathrm{0}\\ \mathrm{0}\\ \mathrm{1}\end{array}\right]（6）

式中：l_leg为步行腿的长度；θ_LF、θ_RF、θ_LR、θ_RR分别为左前、右前、左后和右后的步行腿与底盘之间的夹角；R为轮胎半径；z_s为底盘的垂向位移；l_{\mathrm{a}}为前底盘铰接点至底盘重心的纵向距离；l_c为左右底盘铰接点之间的横向距离。

联立式（4）～式（6），可建立底盘姿态、步行腿角度与路面高程信息之间的关系：

w_{\mathrm{L}\mathrm{F}}=z_{\mathrm{s}}-R-l_{\mathrm{l}\mathrm{e}\mathrm{g}}\mathrm{s}\mathrm{i}\mathrm{n}({\theta }_{\mathrm{s}}+{\theta }_{\mathrm{L}\mathrm{F}})-l_{\mathrm{a}}\mathrm{s}\mathrm{i}\mathrm{n}{\theta }_{\mathrm{s}}+

l_{\mathrm{c}}\mathrm{c}\mathrm{o}\mathrm{s}{\theta }_{\mathrm{s}}\mathrm{s}\mathrm{i}\mathrm{n}{\phi }_{\mathrm{s}}/\mathrm{2}（7）

w_{\mathrm{R}\mathrm{F}}=z_{\mathrm{s}}-R-l_{\mathrm{l}\mathrm{e}\mathrm{g}}\mathrm{s}\mathrm{i}\mathrm{n}({\theta }_{\mathrm{s}}+{\theta }_{\mathrm{R}\mathrm{F}})-l_{\mathrm{a}}\mathrm{s}\mathrm{i}\mathrm{n}{\theta }_{\mathrm{s}}-

l_{\mathrm{c}}\mathrm{c}\mathrm{o}\mathrm{s}{\theta }_{\mathrm{s}}\mathrm{s}\mathrm{i}\mathrm{n}{\phi }_{\mathrm{s}}/\mathrm{2}（8）

w_{\mathrm{L}\mathrm{R}}=z_{\mathrm{s}}-R+l_{\mathrm{b}}\mathrm{s}\mathrm{i}\mathrm{n}{\theta }_{\mathrm{s}}-l_{\mathrm{l}\mathrm{e}\mathrm{g}}\mathrm{s}\mathrm{i}\mathrm{n}({\theta }_{\mathrm{L}\mathrm{R}}-{\theta }_{\mathrm{s}})+

l_{\mathrm{c}}\mathrm{c}\mathrm{o}\mathrm{s}{\theta }_{\mathrm{s}}\mathrm{s}\mathrm{i}\mathrm{n}{\phi }_{\mathrm{s}}/\mathrm{2}（9）

w_{\mathrm{R}\mathrm{R}}=z_{\mathrm{s}}-R+l_{\mathrm{b}}\mathrm{s}\mathrm{i}\mathrm{n}{\theta }_{\mathrm{s}}-l_{\mathrm{l}\mathrm{e}\mathrm{g}}\mathrm{s}\mathrm{i}\mathrm{n}({\theta }_{\mathrm{R}\mathrm{R}}-{\theta }_{\mathrm{s}})-

l_{\mathrm{c}}\mathrm{c}\mathrm{o}\mathrm{s}{\theta }_{\mathrm{s}}\mathrm{s}\mathrm{i}\mathrm{n}{\phi }_{\mathrm{s}}/\mathrm{2}（10）

式中：w_LF、w_RF、w_LR、w_RR分别为左前、右前、左后和右后的轮胎对应的路面高程；l_b为后底盘铰接点至底盘重心的纵向距离。

3 UWE轨迹规划方法

为保证UWE在复杂地形条件下的稳定性，提出一种UWE轨迹规划方法。首先利用NURBS曲线构建步行腿在行驶过程中的运动轨迹，然后建立融合多目标、多约束、路面高程信息，以最小化车身姿态偏移与主动调控能耗为目标的非线性轨迹优化模型。

3.1 基于NURBS曲线的轨迹描述

k次NURBS曲线可表述为

p(u)={\displaystyle \sum _{i=\mathrm{0}}^n}{w}_i{d}_i{N}_{i,k}(u)/{\displaystyle \sum _{i=\mathrm{0}}^n}{w}_i{N}_{i,k}(u)（11）

N_{i,k}(u)=\frac{u-u_i}{u_{i+k}-u_i}{N}_{i,k-\mathrm{1}}(u)+\frac{u_{i+k+\mathrm{1}}-u}{u_{i+k+\mathrm{1}}-u_{i+\mathrm{1}}}·N_{i+\mathrm{1},k-\mathrm{1}}(u)

i=1，2，…，n

式中：n为参与曲线构造的控制顶点数量；p（u）为u时刻的步行腿液压缸行程；d_i 为曲线的控制顶点；w_i 为控制顶点i的权重因子；N_i，k （u）为节点矢量 U =（u₀，u₁，…，u_2k+n+1） 定义的k次B样条有理基函数；下标i表示第i个控制顶点；下标k为B样条基函数的次数。

每个基函数在参数区间［u_i，u_i+k+1）内非零，决定了控制顶点在该区间内对曲线形状的影响范围。本文采用规范节点矢量，令u₀=u₁=…=u_k =0， u_k+n+1=u_k+n+2=…=u_2k+n+1=1，对其余节点采用累计弦长参数化方法进行归一化，有

u_i=u_{i-\mathrm{1}}+|p_i-p_{i-\mathrm{1}}|/{\displaystyle \sum _{r=\mathrm{0}}^{n-\mathrm{1}}}|p_i-p_{i-\mathrm{1}}|（12）

i=k+\mathrm{1},k+\mathrm{2},\cdots ,k+n

令R_{i,k}(u)=w_i{N}_{i,k}(u)/{\displaystyle \sum _{j=\mathrm{0}}^n}{w}_j{N}_{j,k}(u)，则式（11）可改写为

p(u)={\displaystyle \sum _{i=\mathrm{0}}^n}{d}_i{R}_{i,k}(u)（13）

规定0/0=0，则P(u)的k阶导数可表示为

p^{(k)}(u)=\frac{\mathrm{1}}{w(u)}(A^{(k)}(u)-{\displaystyle \sum _{i=\mathrm{1}}^k}{C}_k^i{w}^{(k)}(u)p^{(k-i)}(u))（14）

A^{(k)}(u)={\displaystyle \sum _{i=\mathrm{0}}^n}{w}_i{d}_i{N}_{i,k}(u)w^{(k)}(u)={\displaystyle \sum _{i=\mathrm{0}}^n}{w}_i{N}_{i,k}(u)

NURBS曲线通常不经过除首末端点之外的其余控制点，若使NURBS曲线经过给定的位置节点（值型点），需反求控制点。k次NURBS曲线具有n+1个值型点，因此需反求的控制点数为n+k。由时间-位置节点序列可得n+1个方程：

p(u_{i+k})=p_i={\displaystyle \sum _{r=i}^{i+k}}{d}_r{R}_{r,k}(u_{i+k})i=\mathrm{0,1},\cdots ,n（15）

为保证方程有唯一解，需要增加k-1个条件。本文选用k=5，需要添加4个端点条件：

\left.\begin{array}{l}{\dot{p}}_{\mathrm{0}}=\dot{p}(u_k)=v_{\mathrm{0}}\\ {\ddot{p}}_{\mathrm{0}}=\dot{p}(u_k)=a_{\mathrm{0}}\\ {\dot{p}}_n=\dot{p}(u_{n+k})=v_{\mathrm{f}}\\ {\ddot{p}}_n=\dot{p}(u_{n+k})=a_{\mathrm{f}}\end{array}\right\}（16）

式中：v₀、a₀分别为给定的轨迹初始速度、加速度；a_f、a_f分别为给定的轨迹末端速度、加速度。

联立式（14）、式（16），可得由轨迹型值点反求控制点的矩阵方程：

Ad = p（17）

\mathit{A}\in {\mathrm{R}}^{(n+\mathrm{4})\times (n+\mathrm{4})}\mathit{d}=(d_{\mathrm{0}},d_{\mathrm{1}},\cdots ,d_{n+\mathrm{4}})

\mathit{p}=(p_{\mathrm{0}},p_{\mathrm{1}},\cdots ,p_n,v_{\mathrm{0}},a_{\mathrm{0}},v_{\mathrm{f}},a_{\mathrm{f}})

式中： A 为B样条有理基函数组成的系数矩阵。

进而可得控制顶点：

d = A^-1p（18）

将指定的权因子和控制顶点代入式（11）即可得到步行腿的NURBS运动轨迹曲线。通过式（14）求解曲线方程得到的一阶、二阶导数分别对应曲线的速度和加速度。

3.2 目标函数

本文的目标是保证UWE在崎岖地形上行驶时姿态的稳定性并降低主动调控产生的能耗，因此目标函数由两部分组成：

1）UWE姿态的稳定性由底盘在每个工作周期中的垂向位移、俯仰角、侧倾角和垂向加速度定量描述：

J_C=\frac{k_{C\mathrm{2}}}{t_{\mathrm{f}}}{\int }_{\mathrm{0}}^{t_{\mathrm{f}}}|{\ddot{z}}_{\mathrm{s}}(t)|\mathrm{d}t+\frac{k_{C\mathrm{1}}}{t_{\mathrm{f}}}{\int }_{\mathrm{0}}^{t_{\mathrm{f}}}(|{\theta }_{\mathrm{s}}(t)|+|{\phi }_{\mathrm{s}}(t)|+|z_{\mathrm{s}}(t)|)\mathrm{d}t（19）

式中：t_f为工作周期的长度；k_C1、k_C2分别为姿态稳定性和垂向加速度对应的权重系数。

2）能量消耗可通过步行腿液压缸在每个工作周期中的加速度定量描述。液压缸位移与步行腿夹角θ_i 对应，因此液压缸位移可由步行腿角加速度衡量：

J_E={\int }_{\mathrm{0}}^{t_f}({\ddot{\theta }}_{\mathrm{1}}+{\ddot{\theta }}_{\mathrm{2}}+{\ddot{\theta }}_{\mathrm{3}}+{\ddot{\theta }}_{\mathrm{4}})\mathrm{d}t（20）

则目标函数可表示为

J=k_C{J}_C+k_E{J}_E（21）

式中：k_C 、k_E 分别为姿态稳定性目标函数J_C 与能耗目标函数J_E 对应的权重系数。

3.3 约束条件

为获得UWE在运行过程中切实可行的步行腿运动轨迹，优化必须考虑硬件性能、行驶安全等约束。

1）受液压系统最大流量的约束，每条步行腿液压缸的运动速度与加速度需在最小允许值与最大允许值之间：

\left.\begin{array}{c}{C}_{\mathrm{1}}=\mathrm{m}\mathrm{i}\mathrm{n}{v}_{\mathrm{u}}-v_{\mathrm{u}i}(t)\le \mathrm{0}\\ C_{\mathrm{2}}=v_{\mathrm{u}i}(t)-\mathrm{m}\mathrm{a}\mathrm{x}{v}_{\mathrm{u}}\le \mathrm{0}\end{array}\right\}（22）

\left.\begin{array}{c}{C}_{\mathrm{3}}=\mathrm{m}\mathrm{i}\mathrm{n}{\dot{v}}_{\mathrm{u}}-{\dot{v}}_{\mathrm{u}i}(t)\le \mathrm{0}\\ C_{\mathrm{4}}={\dot{v}}_{\mathrm{u}i}(t)-\mathrm{m}\mathrm{a}\mathrm{x}{\dot{v}}_{\mathrm{u}}\le \mathrm{0}\end{array}\right\}（23）

式中：v_u为步行腿液压缸的伸缩速度。

2）为保证运行过程中整车的稳定性与工作的安全性，UWE整车的姿态与车身垂向加速度需要在最小允许值与最大允许值之间：

\left.\begin{array}{c}{C}_{\mathrm{5}}=\mathrm{m}\mathrm{i}\mathrm{n}{\theta }_{\mathrm{s}}-{\theta }_{\mathrm{s}}(t)\le \mathrm{0}\\ C_{\mathrm{6}}={\theta }_{\mathrm{s}}(t)-\mathrm{m}\mathrm{a}\mathrm{x}{\theta }_{\mathrm{s}}\le \mathrm{0}\end{array}\right\}（24）

\left.\begin{array}{c}{C}_{\mathrm{7}}=\mathrm{m}\mathrm{i}\mathrm{n}{\phi }_{\mathrm{s}}-{\phi }_{\mathrm{s}}(t)\le \mathrm{0}\\ C_{\mathrm{8}}={\phi }_{\mathrm{s}}(t)-\mathrm{m}\mathrm{a}\mathrm{x}{\phi }_{\mathrm{s}}\le \mathrm{0}\end{array}\right\}（25）

\left.\begin{array}{c}{C}_{\mathrm{9}}=\mathrm{m}\mathrm{i}\mathrm{n}{\ddot{z}}_{\mathrm{s}}-{\ddot{z}}_{\mathrm{s}}(t)\le \mathrm{0}\\ C_{\mathrm{10}}={\ddot{z}}_{\mathrm{s}}(t)-\mathrm{m}\mathrm{a}\mathrm{x}{\ddot{z}}_{\mathrm{s}}\le \mathrm{0}\end{array}\right\}（26）

3.4 建立优化模型

优化求解前，UWE首先利用车载激光雷达系统扫描地形，提取关键地形特征以得到4个轮胎对应的路面激励向量 w_m =（w_LFm，w_RFm，w_LRm，w_RRm ），m=1，2，…，g，g为轮胎前方路面激励特征点的数量。将获取的路面激励序列代入式（7）~式（10）联立求解，并对θ_s与φ_s进行小角度简化，从而得到车体姿态的运动学模型：

\left.\begin{array}{l}{\theta }_{\mathrm{s}}=\frac{w_{\mathrm{L}\mathrm{R}}-w_{\mathrm{L}\mathrm{F}}-l_{\mathrm{l}\mathrm{e}\mathrm{g}}\mathrm{s}\mathrm{i}\mathrm{n}{\theta }_{\mathrm{L}\mathrm{F}}+l_{\mathrm{l}\mathrm{e}\mathrm{g}}\mathrm{s}\mathrm{i}\mathrm{n}{\theta }_{\mathrm{L}\mathrm{R}}}{l_{\mathrm{a}}+l_{\mathrm{b}}+l_{\mathrm{l}\mathrm{e}\mathrm{g}}\mathrm{c}\mathrm{o}\mathrm{s}{\theta }_{\mathrm{L}\mathrm{F}}+l_{\mathrm{l}\mathrm{e}\mathrm{g}}\mathrm{c}\mathrm{o}\mathrm{s}{\theta }_{\mathrm{L}\mathrm{R}}}\\ {\phi }_{\mathrm{s}}=[l_{\mathrm{l}\mathrm{e}\mathrm{g}}(\mathrm{s}\mathrm{i}\mathrm{n}{\theta }_{\mathrm{L}\mathrm{F}}-\mathrm{s}\mathrm{i}\mathrm{n}{\theta }_{\mathrm{R}\mathrm{F}}+{\theta }_{\mathrm{s}}\mathrm{c}\mathrm{o}\mathrm{s}{\theta }_{\mathrm{L}\mathrm{F}}-\\ {\theta }_{\mathrm{s}}\mathrm{c}\mathrm{o}\mathrm{s}{\theta }_{\mathrm{R}\mathrm{F}})+w_{\mathrm{L}\mathrm{F}}-w_{\mathrm{R}\mathrm{F}}]/l_{\mathrm{c}}\\ z_{\mathrm{s}}=w_{\mathrm{L}\mathrm{F}}+R+l_{\mathrm{l}\mathrm{e}\mathrm{g}}({\theta }_{\mathrm{s}}\mathrm{c}\mathrm{o}\mathrm{s}{\theta }_{\mathrm{L}\mathrm{F}}+\mathrm{s}\mathrm{i}\mathrm{n}{\theta }_{\mathrm{L}\mathrm{F}})+\\ l_{\mathrm{a}}{\theta }_{\mathrm{s}}-l_{\mathrm{c}}/(\mathrm{2}{\phi }_{\mathrm{s}})\end{array}\right\}（27）

UWE步行腿的角度与路面激励对车身姿态均会产生显著影响。鉴于路面激励不可控，为确保UWE运行过程中的稳定性，优化模型选取变量

x_m =（θ_LFm，θ_RFm，θ_LRm，θ_RRm ）（28）

优化变量的取值范围通常由UWE的机械结构决定。如图4所示，整体优化首先利用基于NURBS曲线的轨迹描述方法获得步行腿轨迹的时间序列曲线及其对应的速度和加速度曲线，随后将步行腿轨迹的位移、速度及加速度序列代入运动学模型和优化模型，最后将求解得到的最优变量 x *作为UWE的控制输入。

综上所述，基于运动学模型、约束条件和目标函数，得到优化模型：

\left.\begin{array}{l}\mathrm{f}\mathrm{i}\mathrm{n}\mathrm{d}\mathit{x}\\ \mathrm{m}\mathrm{i}\mathrm{n}J=k_C{J}_C+k_E{J}_E\\ \mathrm{s}.\mathrm{t}.x_{\mathrm{m}\mathrm{i}\mathrm{n}}\le x_{\mathrm{𝓁}m}\le x_{\mathrm{m}\mathrm{a}\mathrm{x}}(l=\mathrm{L}\mathrm{F},\mathrm{R}\mathrm{F},\mathrm{L}\mathrm{R},\mathrm{R}\mathrm{R})\\ C_q\le \mathrm{0}(q=\mathrm{1,2},\cdots ,\mathrm{10})\end{array}\right\}（29）

由目标函数及约束条件的形式可知，上述优化问题为非线性规划问题，因此本文采用遗传算法进行求解。

4 仿真验证

为验证所提方法的有效性，基于Unity和MATLAB进行数学-物理联合仿真。仿真在配备i7-10875H CPU和16 GB RAM的计算机上进行。

4.1 典型工况描述

为真实模拟灾后废墟、林地和山丘等传统工程机械难以进入的复杂作业环境，构建4种短距离典型障碍、一段长距离山地随机地形，以全面评估步履式挖掘机在复杂环境下的通过能力、稳定性及适应性。

4种典型障碍考察车辆在不同高度差、倾斜状态和复杂地形中的通过能力和稳定性。

1）工况1模拟车辆左前与右后车轮同时越障。这种情况易导致其他2个车轮离地，无法顺利通过障碍物，甚至发生翻车。图5a所示为车辆驶过前后放置的单边桥。单边桥的长度为10 m，高度为0.8 m，单边桥过渡段长度为2 m。

2）工况2模拟易导致车辆的侧倾或侧翻的单侧车轮连续越障。图5b所示为左轮连续越过2个梯形障碍。梯形障碍的长度为4.2m，高度为0.8m，梯形过渡段长度为2.4m。

（3）工况3模拟两侧车轮连续越过两侧不同大小的波浪形障碍，如图5c所示，其中，大障碍长11 m、高1 m，小障碍长8 m、高0.6 m。

4）工况4模拟车辆在斜坡上越过波浪形障碍，如图5d所示，其中，斜坡的坡度为15%，大波浪形障碍长8 m、高0.4 m，小波浪形障碍长4 m、高0.4 m，大小波浪形障碍的波峰与波谷相互交错布置。

4种工况障碍物的最大高度为0.8 m，最小高度为0.4 m，而大多数过障碍研究中的障碍物高度普遍为0.1 m。因此，4种工况的障碍物较为复杂，具有更高的挑战性。

长距离的非结构化山地随机地形为全长45.5 m、总体下倾的地形，最大高度差为3.1 m。起始段为一个垂直下降2.1 m、坡度22.8%、长9 m的下坡。沿途不均匀分布多个小坡，最大高度为0.8 m，坡度约为9.1%。

4.2 联合仿真结果

选取徐工集团ET120步履式挖掘机作为原型机，验证所提优化框架的性能。联合仿真中，UWE的结构参数如表1所示，优化过程中约束条件的上下限取值如表2所示。

使用MATLAB中的遗传算法求解多目标优化模型，其中，种群规模设置为200，最大代数设置为500。将获取的运动轨迹作为系统的输入，传输到Unity搭建的物理仿真环境。UWE自主作业过程中的安全性更重要，因此将式（29）中的权重系数k_C 、k_C1、k_C2、k_E 分别设为0.7、0.6、0.4和0.3。在随机地形工况中，路程较长且总体为下坡，因此不考虑垂向位移，将J_C 修改为

J_C=\frac{k_{C\mathrm{1}}}{t_{\mathrm{f}}}{\int }_{\mathrm{0}}^{t_{\mathrm{f}}}(|{\theta }_{\mathrm{s}}(t)|+|{\phi }_{\mathrm{s}}(t)|)\mathrm{d}t+\frac{k_{C\mathrm{2}}}{t_{\mathrm{f}}}{\int }_{\mathrm{0}}^{t_{\mathrm{f}}}|{\ddot{z}}_{\mathrm{s}}(t)|\mathrm{d}t（30）

图6展示了5种工况下UWE越障时的车身姿态。未受控状态下，UWE的侧倾显著，存在侧翻的风险，轮胎明显离地，这增加了打滑的可能性，影响越障能力。受控状态下，UWE能更好维持车体姿态的稳定，有效应对各种障碍，提升行驶安全性。

不同工况下的车身姿态如图7所示。优化后的步行腿运动轨迹可保证UWE在各种工况下保持车身平稳，俯仰角、侧倾角均得到有效抑制并趋近于0。障碍物较高时，底盘需要升起以防止碰撞，这导致垂向位移在某些时段出现较大的波动。接触和脱离障碍物时，车身姿态可能出现小幅波动的主要原因是某些障碍物起伏较大，导致车轮与其发生明显的碰撞。

不同工况下，俯仰角、侧倾角和垂向位移相对于不受控情况下的均值减小率及峰值减小率（以未控制状态（无控制输入）为基准）如表3所示。均值减小率的最大值为89.23%，最小值为61.42%。峰值减小率的最大值为94.81%，最小值为40.01%。均值减小率的平均值为80.58%，峰值减小率的平均值约为76.37%。底盘升高时，车身姿态变化较大，导致均值减小率略有下降。本实验配套的视频（见https：∥www.bilibili.com/video/BV1fqmUYZEYn/spm_id_from=33.1387.

homepage.video_card.click&vd_source=8ab6b37-

bb5204074da104418cbeb0dac）全面记录了步履式挖掘机通过随机地形的全过程。所提方法在各工况下均能有效提高UWE越障的稳定性，显著减小不同地形条件下底盘的俯仰与侧倾角，满足实际工程应用对稳定性的要求，适用于复杂地形的施工作业、救灾和应急救援。

5 实车实验

为评估所提策略的工程适用性与安全性能，在标准化测试场构建了具有地形扰动特征的实验环境。考虑过高的障碍会引起重型车辆不可控的倾覆，选取中等尺寸障碍物（高度0.11m）开展实验。该障碍物在确保系统核心功能可测性的同时可有效规避倾覆事故的风险。

如图8所示，UWE样机系统集成了多种传感器，实现了对环境的感知、位姿的精确测量以及液压系统的实时状态监测。LiDAR精确感知周围地形，生成实时的路面高程信息，为后续决策提供关键的环境信息。IMU通过实时监测UWE的姿态（包括俯仰角度和侧倾角）变化，确保在复杂地形中保持正确的工作姿态。GNSS提供高精度的位置信息，有助于实现UWE的全局定位。为实时监控步行腿的运动状态，底盘液压缸上安装了位移传感器。所有传感器的数据传输至工控机，并通过笔者开发的非线性轨迹规划算法进行集成处理。

为验证所提控制策略的有效性，研究设置了两种对比工况：①受控模式即开启提出的自适应姿态控制算法；②未受控模式即关闭主动控制功能，仅保留基础运动控制。通过对比分析两种模式下UWE在越障过程的动态响应，定量评估所提控制策略在实际应用中的效果与工程可行性。

图9展示了UWE跨越障碍物的动态过程。实验结果表明，车轮与障碍物接触时，未受控车体出现明显的俯仰和侧倾，表现出较差的姿态稳定性。启用所提算法后，系统通过实时调节步行腿液压缸，有效抑制了车体姿态的波动，显著提高了越障的平顺性。实验视频（https：∥www.bilibili.com/video/BV1Tao2YoEEE/spm_id_from=333.

1387.homepage.video_card.click&vd_source=8ab6b37bb5204074da104418cbeb0dac）记录了挖掘机在实验场地与虚拟仿真环境交互下的运行全过程。

为定量评估所提策略的有效性，监测了UWE越障时俯仰角和侧倾角的变化。如图10所示，受控模式下，车体姿态角的波动幅度显著减小；未受控模式下，车体出现更大幅度的振荡，存在较高的失稳风险。

与未受控模式相比，受控模式下的最大俯仰角和侧倾角分别减小63.22%和70.34%。这一结果表明，步行腿液压缸的实时调节机制在维持车体姿态稳定方面具有显著效果，说明该机制即使在应对较小障碍时仍能发挥积极作用。

实地实验结果证实，所提策略可显著提高UWE通过地形变化区域时的安全性和稳定性。通过步行腿液压缸的实时调节，系统能有效抑制车体姿态的波动，降低倾覆风险。虽然实验选用较小障碍物以确保安全性，但控制性能的显著提升表明该算法具备应对更复杂地形的潜力，后续可通过渐进式测试方法进一步验证其在更具挑战性工况下的适用性。

6 结论

为提高自主作业过程中的安全性，提出一种无人步履式挖掘机开发框架，并开发了一种多目标优化的车身稳定性控制方法。基于闭环矢量方程推导了底盘的七自由度车身-地形耦合运动学模型，利用NURBS曲线构建了步行腿升降液压缸的运动轨迹。为最小化UWE作业过程中的姿态偏差和能耗，开发了一种集成多目标、多约束和地形信息的非线性轨迹规划模型。最后，在物理仿真环境中进行了一系列实验，验证了所提方法的优越性和有效性。结果表明，所提方法可使整体性能平均提高（平均值减小率）超过80%，在提升车身稳定性和适应复杂地形条件方面具有显著优势，展示了其在实际工程应用中的潜力。

参考文献

[1] 张云， 张杰， 魏宇明. 国内外步履式挖掘机的发展及现状［J］. 建设机械技术与管理， 2020， 33（3）： 67-70.

[2] ZHANG Yun， ZHANG Jie， WEI Yuming. Development and Present Situation of Walking Excavators at Home and Abroad［J］. Construction Machinery Technology & Management， 2020， 33（3）： 67-70.

[3] 张云. 一种步履式挖掘机的设计与研制［D］. 西安： 长安大学， 2016.

[4] ZHANG Yun. Design and development of a walking excavator［D］. Xi'an： Changan University， 2016.

[5] BABU A， KIRCHNER F. Terrain Adaption Controller for a Walking Excavator Robot Using Deep Reinforcement Learning［C］∥2021 20th International Conference on Advanced Robotics （ICAR）. Ljubljana， 2021： 64-70.

[6] 张鑫. 智能型步履式挖掘机动态稳定性分析［D］. 大庆： 东北石油大学， 2014.

[7] ZHANG Xin. Dynamic stability analysis of intelligent walking excavator［D］. Daqing： Northeast Petroleum University， 2014.

[8] BABU A， DANTER L C， WILLENBROCK P， et al. ARTER： a Walking Excavator Robot for Autonomous and Remote Operations［J］. At - Automatisierungstechnik， 2022， 70（10）： 876-887.

[9] DONOSO F A， AUSTIN K J， McAREE P R. How Do ICP Variants Perform when Used for Scan Matching Terrain Point Clouds？［J］. Robotics and Autonomous Systems， 2017， 87： 147-161.

[10] SOROKIN A. Post-repair Testing of the Walking Excavators' Electrical Machines［J］. Earth Sciences and Subsoil Use， 2020， 43（1）： 103-110.

[11] PIZETTA I H B， SANTOS BRANDÃO A， SARCINELLI-FILHO M. Control and Obstacle Avoidance for an UAV Carrying a Load in Forestal Environments［C］∥2018 International Conference on Unmanned Aircraft Systems （ICUAS）. IEEE， 2018： 62-67.

[12] PEVZNER L D， KISELEV N A. Automatic Control System for Walking Dragline Excavator Digging［J］. Mining Science and Technology （Russia）， 2022， 7（1）： 57-65.

[13] GOLBASI O， SAHINER S F. Simulation-based Optimization of Workforce Configuration for Multi-division Maintenance Departments［J］. Computers & Industrial Engineering， 2024， 188： 109880.

[14] 陈晋市， 张淼淼， 毕秋实， 等. 面向自主作业的挖掘机多目标最优挖掘运动规划［J］. 机械工程学报， 2022， 58（7）： 237-245.

[15] CHEN Jinshi， ZHANG Miaomiao， BI Qiushi， et al. Multi-objective Optimal Excavator Movement Planning for Autonomous Operation［J］. Journal of Mechanical Engineering， 2022， 58（7）： 237-245.

[16] DONOSO F A， AUSTIN K J， McAREE P R. Three New Iterative Closest Point Variant-methods that Improve Scan Matching for Surface Mining Terrain［J］. Robotics and Autonomous Systems， 2017， 95： 117-128.

[17] PALMER A W， HILL A J， SCHEDING S J. Methods for Stochastic Collection and Replenishment （SCAR） Optimisation for Persistent Autonomy［J］. Robotics and Autonomous Systems， 2017， 87： 51-65.

[18] 王福德， 寇智慧， 张云. 基于ADAMS的灾害救援机器人姿态规划与越障分析［J］. 建筑机械， 2014（11）： 71-74.

[19] WANG Fude， KOU Zhihui， ZHANG Yun. Attitude Planning and Obstacle Crossing Analysis of Disaster Rescue Robot Based on ADAMS［J］. Construction Machinery，2014（11）： 71-74.

[20] GEHRING C， COROS S， HUTTER M， et al. Practice Makes Perfect： an Optimization-based Approach to Controlling Agile Motions for a Quadruped Robot［J］. IEEE Robotics & Automation Magazine， 2016， 23（1）： 34-43.［LinkOut］

[21] 王福德.一种新型六轮全地形移动步履式挖掘机的设计与研究［D］.沈阳：沈阳理工大学，2012.

[22] WANG Fude. Design and Research of a New Six-wheel All-terrain Mobile Walking Excavator［D］. Shenyang： Shenyang University of Technology， 2012.

[23] 刘鹏虎， 张勇， 张强. 液压挖掘机工作装置的动力学分析及控制［J］. 中国工程机械学报， 2007， 5（1）： 72-74.

[24] LIU Penghu， ZHANG Yong， ZHANG Qiang. Dynamical Analysis and Control for Manipulator of Hydraulic Excavator［J］. Chinese Journal of Construction Machinery， 2007， 5（1）： 72-74.

[25] RAIBERT M， BLANKESPOOR K， NELSON G， et al. BigDog， the Rough-terrain Quadruped Robot［J］. IFAC Proceedings Volumes， 2008， 41（2）： 10822-10825.

[26] HUTTER M， GEHRING C， HÖPFLINGER M A， et al. Toward Combining Speed， Efficiency， Versatility， and Robustness in an Autonomous Quadruped［J］. IEEE Transactions on Robotics， 2014， 30（6）： 1427-1440.

[27] HUTTER M， LEEMANN P， STEVSIC S， et al. Towards Optimal Force Distribution for Walking Excavators［C］∥2015 International Conference on Advanced Robotics （ICAR）. Istanbul， 2015： 295-301.

[28] HUTTER M， LEEMANN P， HOTTIGER G， et al. Force Control for Active Chassis Balancing［J］. IEEE/ASME Transactions on Mechatronics， 2017， 22（2）： 613-622.

[29] ZHANG Tianci， FU Tao， SONG Xueguan， et al. Multi-objective Excavation Trajectory Optimization for Unmanned Electric Shovels Based on Pseudospectral Method［J］. Automation in Construction， 2022， 136： 104176.

[30] FU Tao， ZHANG Tianci， Yilin LYU， et al. Digital Twin-based Excavation Trajectory Generation of Uncrewed Excavators for Autonomous Mining［J］. Automation in Construction， 2023， 151： 104855.

[31] ZHANG Tianci， FU Tao， NI Tao， et al. Data-driven Excavation Trajectory Planning for Unmanned Mining Excavator［J］. Automation in Construction， 2024， 162： 105395.

[32] BJELONIC M， SANKAR P K， BELLICOSO C D， et al. Rolling in the Deep – Hybrid Locomotion for Wheeled-legged Robots Using Online Trajectory Optimization［J］. IEEE Robotics and Automation Letters， 2020， 5（2）： 3626-3633.

[33] SUN Jingyuan， YOU Yangwei， ZHAO Xuran， et al. Towards More Possibilities： Motion Planning and Control for Hybrid Locomotion of Wheeled-legged Robots［J］. IEEE Robotics and Automation Letters， 2020， 5（2）： 3723-3730.

[34] JELAVIC E， HUTTER M. Whole-body Motion Planning for Walking Excavators［C］∥2019 IEEE/RSJ International Conference on Intelligent Robots and Systems （IROS）. Macau， 2019： 2292-2299.

[35] JELAVIC E， BERDOU Y， JUD D， et al. Terrain-adaptive Planning and Control of Complex Motions for Walking Excavators［C］∥2020 IEEE/RSJ International Conference on Intelligent Robots and Systems （IROS）. Las Vegas， 2020： 2684-2691.

[36] JELAVIC E， FARSHIDIAN F， HUTTER M. Combined Sampling and Optimization Based Planning for Legged-wheeled Robots［C］∥2021 IEEE International Conference on Robotics and Automation （ICRA）. Xi'an， 2021： 8366-8372.［LinkOut］

[37] ASPRAGATHOS N A， DIMITROS J K. A Comparative Study of Three Methods for Robot Kinematics［J］. IEEE Transactions on Systems， Man， and Cybernetics Part B， Cybernetics， 1998， 28（2）： 135-145.［PubMed］

基金资助

国家自然科学基金(U24A6008)

河北省自然科学基金(E2024203257)

河北省自然科学基金(E2024203065)

河北省高等学校科学技术研究项目(BJ2025218)