CN113671957A - 星球熔岩管探测机器人的控制方法 - Google Patents

Abstract

本发明涉及星球探测技术领域，具体而言，涉及一种星球熔岩管探测机器人的控制方法。该星球熔岩管探测机器人的控制方法包括：获取预测控制模型和星球熔岩管探测机器人在当下时刻的状态量；通过预测控制模型对当下时刻的状态量进行迭代，获得下一时刻的预测状态量；获取星球熔岩管探测机器人的运动约束条件，通过运动约束条件对预测状态量进行约束，获得输入增量；通过输入增量控制星球熔岩管探测机器人追踪目标轨迹。由此，通过运动约束条件对预测状态量进行约束获得输入增量，使输入增量可以约束于安全范围内，避免产生运动失效的情况，从而增加了对星球熔岩管探测机器人的控制稳定性。

Description

星球熔岩管探测机器人的控制方法

技术领域

本发明涉及星球探测技术领域，具体而言，涉及一种星球熔岩管探测机器人的控制方法。

背景技术

星球熔岩管探测机器人主要用于在星球表面进行考察。现有技术中，星球熔岩管探测机器人主要通过PID控制算法或神经网络控制算法对运动路径进行追踪，但是，这些算法对参数和环境的依赖程度较高，当星球熔岩管探测机器人在星球的表面追踪目标轨迹时，星球熔岩管探测机器人容易受到路面因素及机器人动力因素的影响而产生运动失效的情况，从而影响了星球熔岩管探测机器人的控制稳定性。

发明内容

本发明解决的问题是如何提高对星球熔岩管探测机器人的控制稳定性。

为解决上述问题，本发明提供一种星球熔岩管探测机器人的控制方法，包括获取预测控制模型和星球熔岩管探测机器人在当下时刻的状态量；通过所述预测控制模型对当下时刻的所述状态量进行迭代，获得下一时刻的预测状态量；获取星球熔岩管探测机器人的运动约束条件，通过所述运动约束条件对所述预测状态量进行约束，获得输入增量；通过所述输入增量控制所述星球熔岩管探测机器人追踪目标轨迹。

可选地，所述获取星球熔岩管探测机器人的运动约束条件，通过所述运动约束条件对所述预测状态量进行约束，获得输入增量包括：对当前时刻的状态量和下一时刻的所述预测状态量之和进行二次规划，使下一时刻的所述预测状态量约束至所述运动约束条件内，对约束后的预测状态量进行矩阵转换获得所述输入增量。

可选地，所述运动约束条件包括运动速度约束条件、运动加速度约束条件、结构干涉约束条件和运动区域约束条件中的至少一种。

可选地，通过所述输入增量控制所述星球熔岩管探测机器人追踪目标轨迹包括：获取抗打滑力位混合控制律；将所述输入增量代入所述抗打滑力位混合控制律，获得所述星球熔岩管探测机器人的驱动机构的控制增量；基于所述控制增量控制星球熔岩管探测机器人的驱动机构运行。

可选地，所述获取抗打滑力位混合控制律包括：获取所述星球熔岩管探测机器人的牵引力系数、动力学模型和牵引力系数约束条件；将所述牵引力系数约束至所述牵引力系数约束条件内获取期望牵引力系数；将所述期望牵引力系数代入所述动力模型获得期望拉绳拉力值；通过所述期望拉绳拉力值获得抗打滑力位混合控制律。

可选地，所述抗打滑力位混合控制率为：τ_D＝T_D+τ₁+τ₂：其中，T_D为所述星球熔岩管探测机器人的力矩前馈项；τ₁为所述星球熔岩管探测机器人基于位置偏差的PD控制项；τ₂为所述星球熔岩管探测机器人基于期望拉绳拉力值偏差的PD控制项。

可选地，所述预测控制模型为MPC模型。

与现有技术相比，本发明所述的星球熔岩管探测机器人的控制方法具有的有益效果是：

本发明通过获取所述预测控制模型和所述星球熔岩管探测机器人在当下时刻的状态量，使所述星球熔岩管探测机器人可以通过所述预测控制模型对所述当下时刻的状态量进行迭代获得下一时刻的预测状态量；通过获取所述车辆运动约束条件，并通过所述运动约束条件对所述预测状态量进行约束获得输入增量，使所述输入增量可以约束于安全范围内，避免产生运动失效的情况，从而增加了对所述星球熔岩管探测机器人的控制稳定性，通过将所述输入增量控制所述星球熔岩管探测机器人追踪目标轨迹，实现了对目标轨迹的追踪。

本发明还提供一种星球熔岩管探测机器人控制装置，包括获取模块，用于获取所述星球熔岩管探测机器人在当下时刻的状态量和运动约束条件；预测模块，用于通过所述预测控制模型对当下时刻的所述状态量进行迭代，输出下一时刻的预测状态量；修改模块，用于通过所述运动约束条件对所述预测状态量进行约束，获得输入增量；控制模块，用于通过所述输入增量控制所述星球熔岩管探测机器人追踪目标轨迹。所述星球熔岩管探测机器人控制装置具有所述星球熔岩管探测机器人的控制方法具有的有益效果，在此不再赘述。

本发明还提供一种星球熔岩管探测机器人，包括存储有计算机程序的计算机可读存储介质和处理器，当所述计算机程序被所述处理器读取并运行时，实现如上所述的星球熔岩管探测机器人的控制方法。所述星球熔岩管探测机器人的控制方法具有的有益效果，在此不再赘述。

本发明还提供一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，当所述计算机程序被处理器读取并运行时，实现如上所述的星球熔岩管探测机器人的控制方法。

附图说明

图1为本发明实施例中的星球熔岩管探测机器人的控制方法的流程框图；

图2为本发明实施例中的星球熔岩管探测机器人的模型俯视图；

图3为本发明实施例中的星球车等效单车模型示意图；

图4为本发明实施例中的星球车的受力示意图。

具体实施方式

为使本发明的上述目的、特征和优点能够更为明显易懂，下面结合附图对本发明的具体实施例做详细的说明。

在本说明书的描述中，参考术语“实施例”、“一个实施例”和“一个实施方式”等的描述意指结合该实施例或实施方式描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示实施方式中。在本说明书中，对上述术语的示意性表述不一定指的是相同的实施例或实施方式。而且，描述的具体特征、结构、材料或者特点可以在任何的一个或多个实施例或实施方式以合适的方式结合。

本发明实施例提供一种星球熔岩管探测机器人的控制方法，如图1所示，包括如下步骤：

S1：获取预测控制模型和星球熔岩管探测机器人在当下时刻的状态量；

S2：通过所述预测控制模型对所述当下时刻的状态量进行迭代，获得下一时刻的预测状态量；

S3：获取星球熔岩管探测机器人的运动约束条件，通过所述运动约束条件对所述预测状态量进行约束，获得输入增量；

S4：通过所述输入增量控制所述星球熔岩管探测机器人追踪目标轨迹。

具体地，所述获取预测控制模型包括：构建所述星球熔岩管探测机器人在松软或硬质平地上的运动学模型。在一种实施方式中，如图2至图3所示，所述星球熔岩管探测机器人为轮系机器人，所述星球熔岩管探测机器人包括两个前轮和一个后轮，由于星球熔岩管探测机器人以阿克曼转向的方式实现在星球表面的转动运动，通过两个前轮的差速运动改变前轮支架的转角(δ)及前轮合速度(V_f)以实现星球熔岩管探测机器人的转向。由此，可以将星球熔岩管探测机器人的前轮等效为单个车轮，获得星球熔岩管探测机器人的单车等效模型。基于所述星球熔岩管探测机器人的等效模型，可以将等效轮速度表示为：

式中，v_f为等效轮的速度；J_e为等效轮与两个前轮的雅克比矩阵；

为车轮的转速；

其中，J_e为等效轮与两个前轮的雅克比矩阵，表示为：

式中：l₁为前轮距离前轮支架中心得距离；为r_s车轮的等效剪切半径。

其中，

为车轮的转速，表示为：

式中，

和

(rad/s)分别为星球熔岩管探测机器人的两个前轮的车轮转速；

(rad/s)为星球熔岩管探测机器人的后轮速度。

在其它的实施方式中，所述星球熔岩管探测机器人的前后轮可以均有两个，依据式

可分别获得前轮和后轮等效轮的速度。

可选地，构建所述星球熔岩管探测机器人在松软或硬质平地上的运动学模型还包括：利用滑转率修正所述等效轮的速度。

由车轮实际前进线速度与理论圆周速度可以得到车轮的滑转率为：

式中，s_i为第i个车轮的滑转率；r_s为第i个车轮的等效剪切半径；

为第i个车轮的实际转速；

为第i个车轮的转速。

将滑转率矩阵定位为：

s＝diag(s₁,s₂,s₃)；

式中，s₁和s₂分别为星球熔岩管探测机器人的两个前轮的车轮滑转率；s₃为星球熔岩管探测机器人的后轮的滑转率。

在考虑了车轮滑转率对车轮实际速度的影响下，可以将等效轮的速度表示为：

式中，s为星球熔岩管探测机器人车轮的滑转率矩阵；J_e为等效轮与两个前轮的雅克比矩阵；

为车轮的转速。

在星球熔岩管探测机器人前进的过程中，由于土壤的形变，车轮会相对地面滑动，产生滑转现象，使星球熔岩管探测机器人的实际前进速度与理论前行速度存在误差，降低了星球熔岩管探测机器人运行学模型的对星球熔岩管探测机器人状态量反馈信息的精准度；通过在运动学模型中限定滑转率对车轮实际转速的影响，可以提高运动模型对星球熔岩管探测机器人实际位资量反馈信息的精准度。

构建所述星球熔岩管探测机器人在松软或硬质平地上的运动学模型还包括：将等效轮速度方程转换为在星球熔岩管探测机器人坐标系下的星球熔岩管探测机器人质心速度方程。如图3所示，根据所述星球熔岩管探测机器人的单车等效模型，将星球熔岩管探测机器人的转向中心点定义为O点，根据车辆转向几何关系，可以得到星球熔岩管探测机器人的转向角速度和质心与转向中心O点的距离。

其中，星球熔岩管探测机器人的转向角速度可以表示为：

式中，ω_o为星球熔岩管探测机器人的转向角速度；V_f为等效轮的速度；δ为前轮支架的转角；l₂为前轮等效轮距离车辆质心的距离；l₃为后轮等效轮距离车辆质心的距离。

其中，星球熔岩管探测机器人的质心与转向中心O点的距离可以表示为：

式中，l₀为质心与转向中心O点的距离；l₂为前轮等效轮距离车辆质心的距离；l₃为后轮等效轮距离车辆质心的距离；δ为前轮支架的转角。

由转向角速度ω_o和质心与转向中心O点的距离l_o，可以将车辆质心在探测车坐标系下的速度表示为：

v_R＝^RT_fv_f；

式中，^RT_f为探测车在平地运动时等效轮速度与车辆质心速度的转换矩阵：v_R为车辆质心在探测车坐标系Σ_R下的速度；v_f为等效轮的实际速度；其中，^RT_f表示为：

式中，l₁为前轮轮心与前轮支架对称面的距离；l₂为等效轮轮心与车体质心的水平距离；l₃为后轮轮心与车体质心的水平距离。

构建所述星球熔岩管探测机器人在松软或硬质平地上的运动学模型还包括：构建世界坐标系下的星球熔岩管探测机器人运动学模型。

依据星球熔岩管探测机器人坐标系下的星球熔岩管探测机器人质心速度方程，则可以得到星球熔岩管探测机器人的运动学模型为：

式中，^WT_R为探测车坐标系映射到世界坐标系下的转化矩阵；^RT_f为探测车在平地运动时等效轮速度与车辆速度的转换矩阵；J_e为等效轮与两个前轮的雅克比矩阵；

为车轮的转速；v_R为车辆质心在探测车坐标系下的速度。

将世界坐标系下的车辆位姿状态方程表示为：ξ＝[x y φ]^T；

式中，x表示星球熔岩管探测机器人在车辆坐标系下X轴方向上的位置，y表示星球熔岩管探测机器人在车辆坐标系下Y轴方向上的位置，φ表示星球熔岩管探测机器人的偏航角。

由此，通过构建世界坐标系Σ_W下的星球熔岩管探测机器人运动学模型以及车辆位姿状态方程可以获得星球熔岩管探测机器人在世界坐标系下的不同时刻的状态量。需要说明的是，所述状态量指的是所述星球熔岩管探测机器人在世界坐标下的状态信息序列，所述状态量可以包括所述星球熔岩管探测机器人的位置信息、速度信息或加速度信息中的至少一种。

所述获取所述星球熔岩管探测机器人的预测控制模型还包括构建所述星球熔岩管探测机器人在斜坡上的运动学模型，获取星球熔岩管探测机器人在斜坡环境下的状态量。所述构建所述星球熔岩管探测机器人在斜坡上的运动学模型包括：建立星球熔岩管探测机器人在星球熔岩管探测机器人坐标系下的星球熔岩管探测机器人质心速度方程。

由于星球熔岩管探测机器人的侧滑速度方向与车轮前进速度方向垂直，在车轮发生侧滑时，车轮的转向角度仍然不会发生改变，即仍可以将星球熔岩管探测机器人的前轮和后轮分别等效为一个等效轮进行简化。通过建立星球熔岩管探测机器人三维运动学模型，可以得到星球熔岩管探测机器人在星球熔岩管探测机器人坐标系下的三维约束方程分别为：

式中，

为星球熔岩管探测机器人前轮的侧向速度；

为星球熔岩管探测机器人后轮的侧向滑移速度。

所述构建所述星球熔岩管探测机器人在斜坡上的运动学模型还包括：将在星球熔岩管探测机器人坐标系下的星球熔岩管探测机器人质心速度方程转换为世界坐标系星球熔岩管探测机器人的速度方程。

建立三维坐标系下的星球熔岩管探测机器人车辆状态方程为：

ξ_R＝[x_R y_R z_R ψ_R θ_R φ_R]^T；

式中：x_R表示星球熔岩管探测机器人在星球熔岩管探测机器人坐标系下在X轴方向上的位置，y_R表示星球熔岩管探测机器人在星球熔岩管探测机器人坐标系下在Y轴方向上的位置，z_R表示星球熔岩管探测机器人在星球熔岩管探测机器人坐标系下在Z轴方向上的位置；ψ_R为滚动角，θ_R为俯仰角，φ_R为航向角。

基于所述星球熔岩管探测机器人车辆状态方程构建星球熔岩管探测机器人坐标系转换为世界坐标系的转换矩阵(^WA_R)；所述转换矩阵(^WA_R)可以表示为：

式中，s表示正弦函数；c表示余弦函数。

基于所述转换矩阵(^WA_R)，将所述星球熔岩管探测机器人在星球熔岩管探测机器人坐标系下的星球熔岩管探测机器人质心速度方程转换为世界坐标下的质心速度：

基于所述等效轮方程、星球熔岩管探测机器人的三维约束方程和所述转换矩阵^WA_R，可以将所述星球熔岩管探测机器人在世界坐标系下的各维度的速度表示为：

则，世界坐标系的星球熔岩管探测机器人的速度状态量可以表示为：

基于所述星球熔岩管探测机器人速度状态量，可以建立星球熔岩管探测机器人的速度方程为：

式中，

为星球熔岩管探测机器人在斜坡运动时等效轮前进速度与车体前进速度的转换矩阵；

为探测星球熔岩管探测机器人在斜坡运动时侧向滑移速度与车体前进速度的转换矩阵；^WA_R为星球熔岩管探测机器人坐标系转换为世界坐标系的转换矩阵。

其中，

为：

为：

建立探测车在世界坐标系下的转向角度方程。由星球熔岩管探测机器人转向角度(φ_R)与航向角(φ)的几何关系可以获得车辆的航向角公式为：

tanφ＝tanφ_R cosψ/cosθ；

对所述航向角公式进行泰勒展开，并保留一次项，可以得到：

依据俯仰角、滚动角在世界坐标系与车体坐标系的几何关系，同理可得到世界坐标系下探测车的转速为：

令星球熔岩管探测机器人在世界坐标系下的状态量方程为：为q＝[ψ θ φ]^T，得到车体的转速表达式为：

其中，

为探测车在斜坡运动时等效轮前进速度与车体转向速度的转换矩阵；

为探测车在斜坡运动时侧向滑移速度与车体转向速度的转换矩阵。

则，在斜坡地形下星球熔岩管探测机器人的运动学模型可以表示为：

由此，通过构建在斜坡环境下的星球熔岩管探测机器人运动学模型，可以获得星球熔岩管探测机器人在斜坡环境下的状态量，提高了星球熔岩管探测机器人在斜坡环境下的状态量信息的精确度，从而提高了星球熔岩管探测机器人在斜坡环境下了控制适应性。

基于所述星球熔岩管探测机器人运动模型构建预测控制模型。具体地，依据所述星球熔岩管探测机器人在平地上的运动学模型或所述星球熔岩管探测机器人在斜坡上的运动学模型，可以将所述星球熔岩管探测机器人的状态量表示为：ξ＝[X Y α]^T。将输入量表示为u＝[v δ]^T；式中，_v为探测车在车体坐标系∑_R中X_R方向的速度。

基于所述星球熔岩管探测机器人状态量和所述输入量可以构建目标函数为：

对所述目标函数进行微分量偏差处理和利用在期望点的泰勒展开，状态微分量可以近似表示为：

对泰勒展开后的目标函数进行线性化处理，并且令线性化后的加权矩阵分别微A和B，可以得到目标函数为：

其中：A为连续状态表达式状态量加权矩阵，可以表示为：

B为连续状态表达式输入量加权矩阵，可以表示为：

为了便于控制器的实际应用，将上述目标函数进行离散化处理，将预测控制模型的控制周期为T，并假设当前时刻下的系统状态偏差为

利用欧拉方法可以将所述目标函数改写为：

将所述目标函数进行离散化处理可以获得：

式中，

为离散化状态表达式状态量加权矩阵，表示为：

为离散化状态表达式输入量加权矩阵；表示为：

对所述目标函数再进行转换可获得：

由此，可以获得星球熔岩管探测机器人在预测控制模型内的车辆状态方程为：

式中，

为离散化状态表达式状态量加权矩阵，表示为：

为离散化状态表达式输入量加权矩阵，表示为：

在步骤S2中，具体地，将当下时刻的所述状态量代入所述星球熔岩管探测机器人状态方程可以获得下一时刻的状态量，这里，所述下一时刻的状态量为下一时刻的预测状态量，通过对所述星球熔岩管探测机器人状态方程进行迭代可以得到对未来N_p个时刻状态量的预测，则所述迭代过程可以表示为：

其中，N_p为预测时域，η(t+i∣t)(i＝0,1,2…N_p)表示在当前时刻t预测得到的t+i(i＝0,1,2…N_p)时的状态量。将上述公式简化为矩阵形式，并引入控制时域N_c，可以获得预测控制模型为：

Y_t＝Ψξ(t∣t)+ΘU_t；

Y_t为星球车在t时刻的状态量矩阵，表示为：

Y_t＝[η(t+1∣t) η(t+2∣t) … η(t+N_p∣t)]^T；

Ψ为状态量加权矩阵，表示为：

U_t为在t时刻的状态量矩阵，表示为：

U_t＝[Δu(t∣t) Δu(t+1∣t) … Δu(t+N_p∣t)]^T；

Θ为状态量加权矩阵，表示为：

由此，通过获取当下时刻的所述星球熔岩管探测机器人状态量，将所述星球熔岩管探测机器人状态量输入所述星球熔岩管探测机器人状态方程可以获得下一时刻的状态量，通过对所述预测控制模型进行迭代实现了对所述星球熔岩管探测机器人运行状态量的预测。

S3：获取星球熔岩管探测机器人的运动约束条件，通过所述运动约束条件对所述预测状态量进行约束，获得输入增量。可选地，所述运动约束条件包括运动约束条件、运动加速度约束条件、结构干涉约束条件或运动区域约束条件中的至少一种。

需要说明的是，所述运动速度约束条件指的是，将星球熔岩管探测机器人的驱动机构的驱动速度和角速度限定至安全的范围内。所述星球熔岩管探测机器人的运动约束条件可以表示为：

式中，v_min为最小运动速度；ω_min为最小角速度；v_max＝min(v_rmax,v_smax)；ω_max＝min(ω_rmax,ω_smax)；式中，v_rmax为最大极限驱动速度；v_smax为最大安全行驶速度v_smax；ω_smax为最大角速度。

当星球熔岩管探测机器人的速度超过极限驱动速度时，星球熔岩管探测机器人会出现晃动，不稳甚至偏移等情况。而且，车轮的转速越大，车轮的滑转率亦会随之增加，降低了星球熔岩管探测机器人牵引性能；通过设置所述星球熔岩管探测机器人的速度约束条件，可以将所述星球熔岩管探测机器人的控制增量限定至安全范围内，增加了星球熔岩管探测机器人的运行稳定性。

具体地，所述加速度约束条件指的是将所述星球熔岩管探测机器人的驱动加速度限定至安全的范围内。所述星球熔岩管探测机器人的加速度约束条件可以表示为：

α_min≤α≤α_max；

式中，α_min为最小运动加速度；α_smax为最大安全行驶加速度。

由于驱动机构过大的驱动加速度会导致严重的滑转滑移现象，导致车辆失稳；而加速度过小时，则难以跟中变化频繁的崎岖路段，将所述加速度约束至一定条件内，一方面可以防止车辆失稳，另一方面，可以增加所述星球熔岩管探测机器人的追踪准确度。

具体地，所述结构干涉约束条件指的是，由于星球熔岩管探测机器人通常通过前轮差速驱动使星球熔岩管探测机器人的前轮支架与底盘相对旋转产生转角，将所述转角限定至一定的范围内，所述结构干涉约束条件可以表示为：-δ_max≤δ≤δ_max；其中，所述-δ_max左向最大转角；δ_max为右向最大转向角度。

由于星球熔岩管探测机器人通常通过前轮差速驱动使星球熔岩管探测机器人的前轮支架与底盘相对旋转产生转角，实现星球熔岩管探测机器人的转向；当星球熔岩管探测机器人的转向转角过大时，星球熔岩管探测机器人的前轮容易与后轮发生干涉，从而对星球熔岩管探测机器人的内部结构造成损伤。通过对所述最大转角进行限定，可以避免前后轮产生干涉，从而增加了所述星球熔岩管探测机器人的运行稳定性。

具体地，所述运动区域约束指的是：定义完全没有障碍物的空间坐标集合为O_3,free；将星球熔岩管探测机器人在空间坐标系中的位置状态定义为(x(t),y(t),z(t))；则可以将运动区域约束表示为：(x(t),y(t),z(t))∈O_3,free。

在星球的探测过程中，通常会遇到许多危险或大坡度的地形，例如：大型岩石障碍区域、易沉陷区域、大坡度岩壁上易倾覆或滑移区域；而在星球熔岩管探测机器人追踪目标轨迹的过程中，星球熔岩管探测机器人实际运行轨迹与目标轨迹不可以避免地会出现一定的偏差，通过对运动区域进行约束，可以避免车辆进行危险区域内，便于对星球熔岩管探测机器人的状态量进行限制。

具体地，可以通过二次型矩阵求和的形式实现对所述预测状态量进行约束，表示为：

其中，Q,R为两个优化目标的权值矩阵。

由于单个控制周期的输入增量在数值上近似等于车体的加速度，因而U_t需要满足运动加速度约束。此外，当前时刻的输入增量值与上一时刻的输入量值之和应该满足运动速度约束与结构干涉约束条件。根据所述运动速度约束条件、所述加速度约束条件、所述结构干涉条件可以获得所述能够获得输入增量值Ut的上下限U_tmax与U_tmin。在车辆运动时，系统状态量应时刻满足运动区域约束，防止发生危险，根据所述车辆状态方程和所述区域约束条件可以得到系统状态量Y_t的上下限值Y_tmax与Y_tmin。

将所述优化目标的权值矩阵代入所述星球熔岩管探测机器人状态方程可以获得：

其中，E＝Ψξ_t-Y_ref，由于E^TQE为常数项，对于优化结果没有影响，可以略去，因而上述表达式可以表示为二次规划问题形式：

min

s.t.U_tmin＜U_t＜U_tmax,t＝0,1,2…

DU_t＝b,t＝0,1,2…；

式中，

运用拉格朗日方法对于上述二次规划问题进行求解，可以得到：

式中，

由此，可以将约束后的所述预测状态量为下一时刻至第N_p时刻的预测状态量的序列，将被约束后的预测状态量U_t矩阵中的第一项与第二项分别作为速度v_f与转角δ的实际作用增量，对所述预测状态量U_t矩阵中的第一项和第二项进行矩阵转换，可以得到车辆的输入增量为：

u(t+1∣t)＝u(t∣t)+Δu(t+1∣t)。

需要说明的是，所述输入增量为表示下一时刻的车辆状态增量的信息序列，所述信息序列至少包含所述车辆的坐标位置增量信息、加速度增量信息和速度增量信息。对所述增量信息进行解耦处理，可以获得下一时刻的驱动机构的驱动增量信息，例如，所述驱动增量信息可以为电机力矩的增加量、转速的增加量或电机转动位置的增加量。

S4：基于所述输入增量控制所述星球熔岩管探测机器人追踪目标轨迹。具体地，将所述输入增量进行解耦处理，得到所述星球熔岩管探测机器人的驱动机构的驱动信号值，所述星球熔岩管探测机器人包括控制装置，所述控制装置获取所述驱动信号值后控制所述驱动机构运行，实现对所述目标轨迹的追踪。

这样设置的好处在于，通过获取所述预测控制模型和所述星球熔岩管探测机器人在当下时刻的状态量，使所述星球熔岩管探测机器人可以通过所述预测控制模型对所述当下时刻的状态量进行迭代获得下一时刻的预测状态量；通过获取所述车辆运动约束条件，通过所述运动约束条件对所述预测状态量进行约束获得输入增量，使所述输入增量可以约束于安全范围内，增加了对所述星球熔岩管探测机器人的控制稳定性，通过将所述输入增量控制所述星球熔岩管探测机器人追踪目标轨迹，实现了对目标轨迹的追踪。

可选地，所述基于所述输入增量控制所述星球熔岩管探测机器人追踪目标轨迹包括：获取抗打滑力位混合控制律；将所述输入增量代入所述抗打滑力位混合控制律，获得所述星球熔岩管探测机器人的驱动机构的控制增量；基于所述驱动机构的控制增量控制所述星球熔岩管探测机器人运行。

所述星球熔岩管探测机器人包括车轮驱动机构和系绳驱动机构，所述车轮驱动机构用于带动车轮转动，所述系绳驱动机构与绳轮驱动连接，所述系绳驱动机构用于带动所述绳轮转动，所述系绳缠绕于所述绳轮上，在所述星球熔岩管探测机器人爬坡的过程中，将固定件插设于坡顶上，系绳的一端与所述固定件连接，所述系绳机构驱动所述绳轮转动，所述绳轮卷绕所述系绳带动所述星球熔岩管探测机器人爬坡，同时，所述车轮驱动机构带动所述车轮转动，为所述星球熔岩管探测机器人的爬坡提供牵引力。

需要说明的是，所述控制律指的是：电机运行过程中的过程控制规律，例如所述控制律可以为位式控制、比例控制、积分控制或微分控制，还可以为比例积分控制或比例微分控制。当以一定的输入值输入所述控制律时，所述控制律以一定的控制规律输出控制信号。

获取所述抗打滑力位混合控制律包括：建立所述星球熔岩管探测机器人的力矩表达式。各个车轮在前进方向上的挂钩牵引力大小能够通过车轮中的力传感器获得，令其为

参考上文星球熔岩管探测机器人运动学建模的过程，可以将探测车的两个前轮看成一个等效轮，其等效挂钩牵引力为两前轮挂钩牵引力之和，建立所述星球熔岩管探测机器人的力矩表达式为：

根据达朗贝尔原理，探测车在机器人坐标系Σ_R中的力平衡方程可以表示为：

式中：m_R为探测车整车质量；I_R为探测车转动惯量。

将二维车体坐标系Σ_R下探测车的状态变量表示为ξ_R＝[x_R y_R φ_R]^T，可以得到星球熔岩管探测机器人在平地上的动力学模型为：

其中，T_D为输入力矩矩阵，表示为：

式中，

分别指两前轮的输入力矩；

指的是后轮的输入力矩；

T_De为额外驱动力矩矩阵，表示为：

分别指两前轮的额外驱动力矩；

指的是后轮的额外驱动力矩。M(ξ_R)为质量矩阵，表示为：

为向心力矩阵，表示为：

B为输入转换矩阵，表示为：

获取所述抗打滑力位混合控制律还包括：基于所述星球熔岩管探测机器人的力矩表达式建立所述星球熔岩管探测机器人在斜坡环境下的动力学模型：

由星球熔岩管探测机器人在斜坡上受力分析可知，在重力作用下，各个车轮易发生前向滑转及侧向滑移的现象。并且由于车体采用阿克曼转向的方式，两前轮绕前轮支架进行转动，转速方向与后轮转速不平行，导致车轮侧滑速度与加速度对于探测车横向及纵向运动的速度与加速度都造成影响。此外，与平面动力学模型不同的是，斜坡上的模型应该包含三个位置参量x_R、y_R、z_R以及三个角度参量ψ_R、φ_R、θ_R。由于整车结构上并不能够对于高度z_R、滚动角ψ_R及俯仰角θ_R进行调节，可对于该变量进行降维，即令三维空间中车体坐标系Σ_R下的状态变量与二维空间中的状态变量一致，为ξ_R＝[x_R y_R φ_R]^T。同时，轮毂会受到土壤给与的侧向阻力，令该阻力为

在实际工作中可以通过力学传感器获取。结合星球熔岩管探测机器人在平地地形下的动力学模型，并加入重力项，能够得到斜坡环境上的星球熔岩管探测机器人的动力学模型为：

其中：

在星球熔岩管探测机器人在悬崖环境中移动时，系绳拉力通过系绳轮传递给整车。令探测车输入力矩矩阵表示为

系绳拉力为f_T，系绳拉力方向与x_R方向保持一致，在实际运动过程中该系绳拉力大小可以通过后轮电机电流值或力传感器获得。此外，后轮轮毂在从动滚动时，会受到地面给与的摩擦阻力。为方便建模，定义后轮轮毂所受到的摩擦阻力为f_r，方向与后轮前进方向相反，将阻力项与拉力项加入星球熔岩管探测机器人的动力学模型中，可以得到斜坡地形中探测车的动力学模型为：

其中：F_r＝[f_r 0 0]^T；F_T＝[f_T 0 0]^T。

由此，可以获得星球熔岩管探测机器人在斜坡环境下的动力学模型，从而可以获得星球熔岩管探测机器人的驱动机构在斜坡环境下的车辆状态信息。

获取所述星球溶岩管探测车的牵引力系数和牵引力系数约束条件；具体地，通过建立探测车挂钩牵引力与法向力的比值来表征牵引特征，即在单位载荷下，土壤提供给探测车的挂钩牵引力大小，基于所述挂钩牵引力可以获得牵引力系数，所述牵引力系数方程可以表示为：

PC＝F_DP/F_N；

式中，F_DP为等效轮的力矩；F_N为土壤最大摩擦系数。

将所述牵引力系数约束至所述牵引力系数约束条件内获取期望牵引力系数；具体地，由于在斜坡环境下，星球熔岩管探测机器人的移动速度较为缓慢，可以忽略惯性力与离心力对于星球熔岩管探测机器人的影响效果，星球熔岩管探测机器人在斜坡环境下的动力学模型可以简化为：

将期望牵引力系数带入所述动力模型获得期望拉绳拉力值。具体地，将简化后的动力学模型代入所述牵引力系数方程可以获得系绳张力的约束条件为：

基于所述系绳张力约束条件可以获得期望拉绳拉力值，所述期望拉绳拉力值可以表示为：

F_Tref＝G(ξ_R)-CF_s-F_r；

式中，F_Tref为期望拉绳拉力值。

由土壤本身的物理性质可知，当牵引力系数大于土壤的最大摩擦系数时，星球熔岩管探测机器人车便无法获得需要的牵引力值，出现滑移现象。通过获得所述期望拉绳拉力值，基于所述期望拉绳拉力值分配系绳拉力与车轮牵引力之间的协调关系，使得牵引力系数可以满足约束条件，从而可以获得星球熔岩管探测机器人在斜坡环境下的抗打滑约束条件。

通过所述期望拉绳拉力值建立抗打滑力位混合控制律。具体地，在绳轮拉动星球熔岩管探测机器人爬坡的过程中，考虑车轮与绳轮协同控制，通过在获得所述探测车系绳张力优化目标的基础上建立车轮驱动机构的抗打滑控制律控制车轮的转动为车辆提供一定的牵引力以防止车轮产生滑移。

所述通过所述期望拉绳拉力值建立抗打滑力位混合控制律包括：建立车轮驱动机构的力矩前馈项，根据所述斜坡环境下的星球熔岩管探测机器人动力学模型可得到星球熔岩管探测机器人的力矩前馈项表达式为：

需要说明的是，一般仅会在斜坡坡度一般大于60°时才会使用系绳驱动与车轮驱动混合驱动的驱动方式，这时，星球熔岩管探测机器人在斜坡环境上进行运动时，额外驱动力矩(T_De)远大于松软土壤的内摩擦角，因而探测车在该类大坡度岩壁上进行运动时额外驱动力矩(T_De)可以近似忽略。

通过将所述输入增量代入所述星球熔岩管探测机器人力矩前馈表达式可以获得所述星球熔岩管探测机器人车轮的力矩前馈项，通过所述力矩前馈项可以为后续对车轮驱动机构控制增量的调整提供基点，与从原点开始调整车轮驱动机构控制增量的方式相比，偏差更小，通过将所述控制增量输入所述车轮驱动机构，可以实现对车轮驱动机构的控制，从而增加了对所述车轮驱动机构的控制精度。

所述通过所述期望拉绳拉力值建立抗打滑力位混合控制律还包括：建立星球熔岩管探测机器人基于位置偏差的PD控制项。通过设置期望轨迹与实际轨迹的偏差值，可以建立基于位置偏差的PD控制项，所述基于位置偏差的PD控制项为：

式中，K_qp为放大比例系数；K_qd为积分时间常数。

由此，通过将所述输入增量代入所述基于位置偏差的PD控制项，可以获得所述车轮驱动机构的第一控制增量调整值，基于所述第一控制增量调整值可以调整所述力矩前馈输出的控制增量，将调整后的控制增量输入所述车轮驱动机构，可以实现对所述车轮驱动机构的控制，偏差更小，增加了对所述车轮驱动机构的控制精度。

所述通过所述期望拉绳拉力值建立抗打滑力位混合控制律还包括：建立车轮驱动机构的基于期望拉绳拉力值偏差的PD控制项。通过设置抗打滑需求中系绳张力优化值F_Tref与实际张力偏差值，可以建立基于期望拉绳拉力值偏差的PD控制项，所述基于期望拉绳拉力值偏差的PD控制项为：

式中，K_Tp为放大比例系数；K_Td为积分时间常数。

由此，通过将所述系绳张力优化值F_Tref与实际张力值的偏差值代入所述基于期望拉绳拉力值偏差的PD控制项，可以获得所述车轮驱动机构的第二控制增量调整值，基于所述第二控制增量调整值可以调整被所述第一控制增量调整值优化后的控制增量，在所述第一控制增量调整值的基础上进一步地优化了所述控制增量，将优化后的控制增量输入所述车轮驱动机构实现了对车轮驱动机构的驱动力与位置偏差的混合控制，从而实现了对所述星球熔岩管探测机器人在斜坡地形上的抗打滑控制。

可选地，所述抗打滑力位混合控制率为：

τ_D＝T_D+τ₁+τ₂：

式中，所述T_D为所述星球熔岩管探测机器人在所述动力学模型下的力矩前馈项；所述τ₁为所述星球熔岩管探测机器人基于位置偏差的PD控制项；所述τ₂为所述星球熔岩管探测机器人基于期望拉绳拉力值偏差的PD控制项。

具体地，分别将所述星球熔岩管探测机器人的力矩前馈项T_D、所述星球熔岩管探测机器人基于位置偏差的PD控制项τ₁和所述星球熔岩管探测机器人基于期望拉绳拉力值偏差的PD控制项τ₂代入所述力位混合控制律，可以获得优化后的控制增量，将所述控制增量转换为电信号输入所述车轮驱动机构可以实现对所述星球熔岩管探测机器人的力位混合控制，从而可以防止星球熔岩管探测机器人在爬坡的过程中车轮产生滑移，从而增加了对目标轨迹追踪精确性。

本发明的另一实施例还提供一种星球熔岩管探测机器人控制装置，包括获取模块、预测模块、修改模块和控制模块；所述获取模块用于获取所述星球熔岩管探测机器人在当下时刻的状态量和运动约束条件；其中，所述当下时刻的状态量值为：基于所述预测模块中包括的车辆参数的车辆运动学模型和所述当下时刻的状态量确定。

所述预测模块用于对所述当下时刻的状态量进行迭代，输出下一时刻的预测状态量；其中，所述预测模块内设置有预测控制模型和车辆参数信息，当所述预测模块收到所述获取模块输出的当下时刻的状态量时，所述预测模块利用所述预测控制模型对所述当下时刻的状态量进行迭代，生成下时刻的预测状态量。

所述修改模块用于通过所述运动约束条件对所述预测状态量进行约束，获得输入增量，其中，所述运动约束条件分别为所述运动约束条件包括运动约束条件、运动加速度约束条件、结构干涉约束条件或运动区域约束条件中的至少一种。

所述控制模块用于通过所述输入增量控制所述星球熔岩管探测机器人追踪目标轨迹。具体地，所述控制模块将所述输入增量进行解耦处理输出电信号值并将所述电信号值输送至所述车轮驱动机构实现对所述星球熔岩管探测机器人的控制。

在本发明的另一实施例中，一种星球熔岩管探测机器人包括存储有计算机程序的计算机可读存储介质和处理器，当所述计算机程序被所述处理器读取并运行时，实现如上所述的星球熔岩管探测机器人的控制方法。

在本发明另一实施例中，一种计算机可读存储介质存储有计算机程序，当所述计算机程序被处理器读取并运行时，实现如上所述的星球熔岩管探测机器人的控制方法。

虽然本公开披露如上，但本公开的保护范围并非仅限于此。本领域技术人员在不脱离本公开的精神和范围的前提下，可进行各种变更与修改，这些变更与修改均将落入本发明的保护范围。

Claims (10)

1.一种星球熔岩管探测机器人的控制方法，其特征在于，包括：

获取预测控制模型和星球熔岩管探测机器人在当下时刻的状态量；

通过所述预测控制模型对当下时刻的所述状态量进行迭代，获得下一时刻的预测状态量；

获取星球熔岩管探测机器人的运动约束条件，通过所述运动约束条件对所述预测状态量进行约束，获得输入增量；

通过所述输入增量控制所述星球熔岩管探测机器人追踪目标轨迹。

2.根据权利要求1所述的星球熔岩管探测机器人的控制方法，其特征在于，所述获取星球熔岩管探测机器人的运动约束条件，通过所述运动约束条件对所述预测状态量进行约束，获得输入增量包括：

对当下时刻的所述状态量和下一时刻的所述预测状态量之和进行二次规划，以使下一时刻的所述预测状态量约束至所述运动约束条件内，对约束后的预测状态量进行矩阵转换获得所述输入增量。

3.根据权利要求2所述的星球熔岩管探测机器人的控制方法，其特征在于，所述运动约束条件包括运动速度约束条件、运动加速度约束条件、结构干涉约束条件和运动区域约束条件中的至少一种。

4.根据权利要求1所述的星球熔岩管探测机器人的控制方法，其特征在于，通过所述输入增量控制所述星球熔岩管探测机器人追踪目标轨迹包括：

获取抗打滑力位混合控制律；

将所述输入增量代入所述抗打滑力位混合控制律，获得所述星球熔岩管探测机器人的驱动机构的控制增量；

基于所述控制增量控制所述星球熔岩管探测机器人的驱动机构运行。

5.根据权利要求4所述的星球熔岩管探测机器人的控制方法，其特征在于，所述获取抗打滑力位混合控制律包括：

获取所述星球熔岩管探测机器人的牵引力系数、动力学模型和牵引力系数约束条件；

将所述牵引力系数约束至所述牵引力系数约束条件内获取期望牵引力系数；将所述期望牵引力系数代入所述动力模型获得期望拉绳拉力值；

通过所述期望拉绳拉力值获得所述抗打滑力位混合控制律。

6.根据权利要求5所述的星球熔岩管探测机器人的控制方法，其特征在于，所述抗打滑力位混合控制率为：

τ_D＝T_D+τ₁+τ₂：

其中，T_D为所述星球熔岩管探测机器人的力矩前馈项，τ₁为所述星球熔岩管探测机器人基于位置偏差的PD控制项，τ₂为所述星球熔岩管探测机器人基于期望拉绳拉力值偏差的PD控制项。

7.根据权利要求1所述的星球熔岩管探测机器人的控制方法，其特征在于，所述预测控制模型为MPC模型。

8.一种星球熔岩管探测机器人控制装置，其特征在于，包括：

获取模块，用于获取所述星球熔岩管探测机器人在当下时刻的状态量和运动约束条件；

预测模块，用于对所述当下时刻的状态量进行迭代，输出下一时刻的预测状态量；

修改模块，用于通过所述运动约束条件对所述预测状态量进行约束，获得输入增量；

控制模块，用于通过所述输入增量控制所述星球熔岩管探测机器人追踪目标轨迹。

9.一种星球熔岩管探测机器人，其特征在于，包括存储有计算机程序的计算机可读存储介质和处理器，当所述计算机程序被所述处理器读取并运行时，实现如权利要求1-7中任一项所述的星球熔岩管探测机器人的控制方法。

10.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质存储有计算机程序，当所述计算机程序被处理器读取并运行时，实现如权利要求1-7中任一项所述的星球熔岩管探测机器人的控制方法。

Images