CN113581285A - 一种液压转向阿克曼型机器人的自动回充控制方法 - Google Patents

Abstract

本发明公开了一种液压转向阿克曼型机器人的自动回充控制方法，包括步骤：(1)分别构建液压转向和电机驱动的输入输出动态模型，并据此构建机器人系统动态预测模型；(2)根据回充任务要求计算得到期望机器人系统状态，并结合步骤(1)构建模型预测控制的目标函数，并依据阿克曼底盘性能建立约束；(3)在当前时刻求解步骤(2)的目标函数得到下一时刻的控制量，并据此控制机器人；(4)重复步骤(3)，直至完成机器人的自动回充控制。本发明在建立系统动态数学模型的时候，包含了两种系统的特性，因此本方法求解得到的控制序列具有可行性，从而保障控制器的稳定性。

Description

一种液压转向阿克曼型机器人的自动回充控制方法

技术领域

本发明涉及移动机器人控制领域，具体涉及一种基于模型预测的液压转向阿克曼型机器人的自动回充控制方法。

背景技术

由于在工程机械领域，大负载的工程机械车辆标准产品常见有履带式以及阿克曼式两种。其中阿克曼型的底盘和汽车类似，都是前轮转向后轮驱动。通常采用四连杆的转向结构，其前轮的转向通常由液压助力控制系统提供。工人在操作的过程中，根据后轮速度的响应情况，实时调整前轮转向，从而控制车辆到达指定地点。

近年来，机器人与自动驾驶领域兴起，在工程机械领域，车辆被改造为移动机器人，用于建筑、消防、运输等各个领域。其中，履带式机器人通常由电机驱动，其运动学与动力学模型相对简单，执行机构电机控制性能较好，控制方法较为成熟。通常上层的路径规划可以使用成熟的差速类底盘的规划方法，底层的跟踪控制一般采用PID控制器进行速度跟踪控制。

然而，液压转向的阿克曼底盘的转向系统由液压系统提供，一般受限于成本，厂商大多采用开关阀通过PWM来模拟比例阀对于液压系统的位置进行直接控制。由于没有在液压缸内设置压力传感器，因此整个控制系统的控制带宽较低，系统的滞后较大，往往需要等待压力建立之后再进行位置调节。此时开关阀通过PWM来控制进一步加剧了位置跟踪性能的滞后。在执行机构为电机的时候，上层的规划控制往往只需考虑简单的运动学约束，忽略动力学系统的滞后。但是对于液压转向的阿克曼底盘来说，系统同时存在两种执行机构，分别是液压转向系统与后轮电机驱动系统，二者的动力学特性截然不同，传统的规划与控制方法难以取得良好的控制效果。

自动回充过程中，由于充电桩的位置和姿态由感知系统实时给出，需要控制系统根据感知获得的信息实时调整车辆的控制命令，如果忽略动力学特性，转向系统和速度控制系统不能相互配合，则最终的到位位置与姿态均难以保证。

发明内容

发明目的：本发明针对上述不足，提出了一种基于模型预测的液压转向阿克曼型机器人的自动回充控制方法，基于模型预测控制的方法，通盘考虑两种驱动系统的特点，通过实验数据构造两个一阶系统来分别描述两种驱动系统的特性，并在此基础上建立系统描述模型方程。

技术方案：

一种液压转向阿克曼型机器人的自动回充控制方法，包括步骤：

(1)分别构建液压转向和电机驱动的输入输出动态模型，并据此构建机器人系统动态预测模型；

(2)根据回充任务要求计算得到期望机器人系统状态，并结合步骤(1)构建模型预测控制的目标函数，并依据阿克曼底盘性能建立约束；

(3)在当前时刻求解步骤(2)的目标函数得到下一时刻的控制量，并据此控制机器人；

(4)重复步骤(3)，直至完成机器人的自动回充控制。

所述步骤(1)中分别构建液压转向和电机驱动的输入输出动态模型具体如下：

(11)一阶系统在复频域下的传递函数φ(s)由下列函数描述：

其中，s为复频域里的变量表示，k为比例系数，τ为一阶系统的时间常数，y(s)是表示的输出信号，x(s)表示系统的输入信号；

则转为时域得到：

其中，t为当前时刻，x(t)表示系统的输入信号，y(t)表示系统的输出信号，

表示输出信号的一阶导数；

整理得到：

(12)构建液压转向的输入输出动态模型；

假设液压转向阿克曼型机器人的控制转向角为δ_control(t)，实际的转向角为δ_actual(t)，k＝1，则根据前述一阶常系数微分方程计算得到：

其中，

表示机器人的实际转向角速度，τ_steering表示机器人转向一阶近似时间常数；

(13)构建电机驱动的输入输出动态模型；

根据前述一阶常系数微分方程计算得到：

其中，

表示机器人的实际前进加速度，v_control表示机器人的前进控制速度，v_actual表示机器人实际的前进速度，τ_velocity表示机器人速度一阶近似时间常数；

(14)设计阶跃函数输入信号：

通过选择不同的c，记录机器人系统的输出y(t)，并通过最小二乘法计算得到使得预测阿克曼型机器人的输出轨迹与理论输出轨迹差异最小的系数τ_steering和τ_velocity。

所述步骤(1)中，构建机器人系统动态预测模型具体为：

机器人的阿克曼底盘在里程计坐标系中的运动描述如下：

其中，L_wheelbase表示机器人的轴距，即后轮到前轮的距离；

根据步骤(1)得到：

定义机器人系统状态变量为x_state＝[x,y,θ,δ,v]，机器人系统的控制量为u＝[v_control,δ_control]，则将其离散化为N时间段之后得到：

x_state(i+1)＝H[x_state(i),u(i)]

其中，x_state(i)表示i时刻的机器人系统状态，x_state(i+1)表示i+1时刻的机器人系统状态；u(i)表示i时刻机器人系统的控制量；i∈[1,2,...,N]；N表示总的预测控制总时间步长。

所述步骤(2)中，根据回充任务要求计算得到期望机器人系统状态具体为：

假设整个回充过程在里程计坐标系下进行，充电桩位置和姿态由机器人感知系统计算给出，由此计算得到当前充电桩在里程计坐标系下的位姿^odomP_dock如下：

^odomP_dock＝[x_dock,y_dock,θ_dock]^T

其中，(x_dock,y_dock)表示充电桩在里程计坐标系下的坐标，θ_dock表示充电桩在里程计坐标系下的朝向；

根据充电桩在里程计坐标系下的位姿计算得到机器人的目标位姿：

^odomP_target＝F(^odomP_dock)＝[x_target,y_target,θ_target]

其中，F(^odomP_dock)为变换函数，根据机器人上充电口的位置将当前充电桩在里程计坐标系下的位姿变换为机器人的目标位姿；

根据自动回充任务要求得到充电桩在里程计坐标系下的目标位姿，据此得到机器人的目标位姿，进而得到期望的机器人系统状态：

x_target(i)＝[^odomP_target,0,0]^T。

所述步骤(2)中，构建的模型预测控制的目标函数，并依据阿克曼底盘性能建立约束具体为：

构建模型预测控制的目标函数：

通过求取控制量序列[u(1),u(2),...u(N)]使得目标函数最小；

其中，控制量序列中各时刻控制量的初始值均为0；Q_N是轨迹末端权重矩阵，对角矩阵Q和R分别是系统状态轨迹权重矩阵和控制轨迹权重矩阵；

建立约束：

其中，X_min,X_max分别表示最小和最大允许的系统状态；ΔX_min,ΔX_max分别表示最小和最大允许的系统状态变化步长；U_min,U_max分别表示最小和最大允许的输入力矩；ΔU_min,ΔU_max分别表示最小和最大允许的直接力矩变化步长。

所述步骤(3)具体为：

在某个控制周期i时刻，控制器求解步骤(2)的目标函数，得到i+1时刻的控制量序列u^＊＝[u(1),u(2),...u(N)]，则取u(1)下发到移动机器人底层控制器中，在该周期内采集系统状态信息，更新系统约束方程，将i时刻的控制量序列代入目标函数中求解得到第i+2时刻的控制量。

有益效果：

1.本发明具有通用性，通过设计不同幅值的阶跃型号记录输入输出数据，对于被控对象没有任何要求，因此具有一般性。

2.本发明在建立系统动态数学模型的时候，包含了两种系统的特性，因此本方法求解得到的控制序列具有可行性，从而保障控制器的稳定性。

3.本发明根据自动回充任务设计目标函数，可以在最终到位的性能上通过权重矩阵进行直观的调节，例如放宽x方向的到位精度要求，收紧对于航向角和y方向的到位精度要求。

4.本发明根据实际系统性能，可以硬性约束控制量的范围，例如转向最大角度以及转向最大角速度约束，最大速度和加速度约束等。保障了自动回充过程的平稳性。

附图说明

图1为本发明基于模型预测的液压转向阿克曼底盘自动回充控制方法的原理简图。

图2为本发明的控制流程图。

具体实施方式

下面结合附图和具体实施例，进一步阐明本发明。

图1为本发明基于模型预测的液压转向阿克曼型机器人的自动回充控制方法的原理简图。如图1所示，本发明的液压转向阿克曼型机器人的自动回充控制方法采用模型预测控制。

图2为本发明的控制流程图。如图2所示，本发明基于模型预测的液压转向阿克曼型机器人的自动回充控制方法包括步骤：

(1)假设整个回充过程在里程计坐标系odom下进行，而且充电桩位置和姿态由机器人感知系统计算给出，则得到当前充电桩在里程计坐标系下的位姿^odomP_dock如下：

^odomP_dock＝[x_dock,y_dock,θ_dock]^T

其中，(x_dock,y_dock)表示充电桩在里程计坐标系下的坐标，θ_dock表示充电桩在里程计坐标系下的朝向；

通常由于机器人配置的不同，相应的机器人大小以及充电口的位置可能不同，所以需要根据充电桩在里程计坐标系下的位姿计算得到机器人的目标位姿：

^odomP_target＝F(^odomP_dock)＝[x_target,y_target,θ_target]

其中，F(^odomP_dock)为变换函数，根据机器人上充电口的位置将当前充电桩在里程计坐标系下的位姿变换为机器人的目标位姿；

(2)构建预测模型：

一阶系统在复频域下的传递函数φ(s)可由下列函数描述：

其中，s为复频域里的变量表示，k为比例系数，τ为一阶系统的时间常数，y(s)是表示的输出信号，x(s)表示系统的输入信号；

则转为时域得到：

其中，t为当前时刻，x(t)表示系统的输入信号，y(t)表示系统的输出信号，

表示输出信号的一阶导数；

整理得到：

通过该一阶常系数微分方程描述了系统输入到输出的整个动态过程；

假设液压转向阿克曼型机器人的控制转向角为δ_control(t)，经过厂商提供的液压控制系统之后，实际的转向角为δ_actual(t)，k＝1，则根据前述一阶常系数微分方程计算得到：

其中，

表示机器人的实际转向角速度，τ_steering表示机器人转向一阶近似时间常数；

同理，该液压转向阿克曼底盘的速度控制系统由于负载较重，存在加速滞后的情况也根据前述一阶常系数微分方程计算得到：

其中，

表示机器人的实际前进加速度，v_control表示机器人的前进控制速度，v_actual表示机器人实际的前进速度，τ_velocity表示机器人速度一阶近似时间常数；

(3)在实际操作过程中，需要根据机器人系统的性能情况设计相应的阶跃函数输入信号：

通过选择不同的c，记录机器人系统的输出y(t)，则通过最小二乘法，可以计算出合适的系数τ_steering和τ_velocity使得预测的输出轨迹(即机器人在恒定的控制指令c的输入下，随着时间变化的运动轨迹)与实验测得的输出轨迹差异最小；

(4)在不考虑转向系统和驱动系统的动态特性假设下，机器人的阿克曼底盘在里程计坐标系odom中的运动描述如下：

其中，L_wheelbase表示机器人的轴距，即后轮到前轮的距离；

由步骤(2)得到：

定义机器人系统状态变量为x_state＝[x,y,θ,δ,v]，机器人系统的控制量为u＝[v_control,δ_control]，则将其离散化为N时间段之后，可简记为：

x_state(i+1)＝H[x_state(i),u(i)]

其中，x_state(i)表示i时刻的机器人系统状态，x_state(i+1)表示i+1时刻的机器人系统状态；u(i)表示i时刻机器人系统的控制量；i∈[1,2,...,N]；N表示总的预测控制总时间步长；

根据自动回充任务要求，机器人感知系统会给出充电桩在里程计坐标系下的目标位姿，并结合步骤(1)得到机器人的目标位姿，进而得到期望的机器人系统状态：

x_target(i)＝[^odomP_target,0,0]^T

(5)构建模型预测控制的目标函数：

其中，第一项的目标是保障该机器人系统最终的稳定性，期望找到合适的控制量序列[u(1),u(2),...u(N)]使得最终的机器人系统状态和期望的状态差最小；控制量序列中各时刻控制量的初始值均为0；Q_N是轨迹末端权重矩阵，主要用于保障系统最终到位的稳定性；第二项的目标是保障机器人系统跟踪过程的平稳性，并且所使用的控制量尽可能小，机器人系统的状态需要尽量接近期望状态；

对角矩阵Q和R分别是系统状态轨迹权重矩阵和控制轨迹权重矩阵，根据需要调节其相对大小，可实现在两个目标之间权衡；另外，可以通过调整Q,R对角线上元素的相对大小，可调节不同状态和控制量的相对重要程度；例如，如果用户倾向于使用转向控制来调节，则可以加大R₁₁元素的权重；或者，如果用户希望尽量减少最终到位的时候y方向和θ方向的误差，则可以减小Q₁₁和Q_N11元素的大小；

下面考虑约束项，首先移动机器人需要满足下列等式约束，

x_state(0)＝X₀

x_state(i+1)＝H[x_state(i),u(i)]

分别表示系统状态的初始值x_state(0)需要赋值成当前系统的状态测量值X₀；

另外是移动机器人的动态系统模型约束，表示在给定状态和输入后，下一个时刻的状态需要满足此方程约束；

除此以外，为保障用户和设备安全，需要加入下列不等式约束，

U_min＜u(i)＜U_max

ΔU_min＜|u(i+1)-u(i)|＜ΔU_max

其中，X_min,X_max分别表示最小和最大允许的系统状态；ΔX_min,ΔX_max分别表示最小和最大允许的系统状态变化步长；U_min,U_max分别表示最小和最大允许的输入力矩；ΔU_min,ΔU_max分别表示最小和最大允许的直接力矩变化步长；两个不等式分别约束系统输入以及系统输入的变化；经过这两个不等式约束之后，求得的系统输入轨迹会变得更为平滑；在实际系统中，如果后面一个约束导致求解时间变长或者招不到可行解，则可以放松相关约束，使其变成系统目标函数的一部分，以降低求解难度；

(6)在某个控制周期i时刻，控制器求解步骤(5)的目标函数，得到i+1时刻的控制量序列u^★＝[u(1),u(2),...u(N)]，则取u(1)下发到移动机器人底层控制器中，在该周期内采集系统状态信息X₀＝[x,y,θ,δ,v]，更新系统约束方程x_state(0)＝X₀，将i时刻的控制量序列代入目标函数中即可求解第i+2时刻的控制量。

(7)重复步骤(6)，则控制系统可以完成液压转向的阿克曼底盘自动回充控制任务。

以上详细描述了本发明的优选实施方式，但是本发明并不限于上述实施方式中的具体细节，在本发明的技术构思范围内，可以对本发明的技术方案进行多种等同变换(如数量、形状、位置等)，这些等同变换均属于本发明的保护范围。

Claims (6)

1.一种液压转向阿克曼型机器人的自动回充控制方法，其特征在于：包括步骤：

(1)分别构建液压转向和电机驱动的输入输出动态模型，并据此构建机器人系统动态预测模型；

(2)根据回充任务要求计算得到期望机器人系统状态，并结合步骤(1)构建模型预测控制的目标函数，并依据阿克曼底盘性能建立约束；

(3)在当前时刻求解步骤(2)的目标函数得到下一时刻的控制量，并据此控制机器人；

(4)重复步骤(3)，直至完成机器人的自动回充控制。

2.根据权利要求1所述的液压转向阿克曼型机器人的自动回充控制方法，其特征在于：所述步骤(1)中分别构建液压转向和电机驱动的输入输出动态模型具体如下：

(11)一阶系统在复频域下的传递函数φ(s)由下列函数描述：

其中，s为复频域里的变量表示，k为比例系数，τ为一阶系统的时间常数，y(s)是表示的输出信号，x(s)表示系统的输入信号；

则转为时域得到：

其中，t为当前时刻，x(t)表示系统的输入信号，y(t)表示系统的输出信号，

表示输出信号的一阶导数；

整理得到：

(12)构建液压转向的输入输出动态模型；

假设液压转向阿克曼型机器人的控制转向角为δ_control(t)，实际的转向角为δ_actual(t)，k＝1，则根据前述一阶常系数微分方程计算得到：

其中，

表示机器人的实际转向角速度，τ_steering表示机器人转向一阶近似时间常数；

(13)构建电机驱动的输入输出动态模型；

根据前述一阶常系数微分方程计算得到：

其中，

表示机器人的实际前进加速度，v_control表示机器人的前进控制速度，v_actual表示机器人实际的前进速度，τ_velocity表示机器人速度一阶近似时间常数；

(14)设计阶跃函数输入信号：

通过选择不同的c，记录机器人系统的输出y(t)，并通过最小二乘法计算得到使得预测阿克曼型机器人的输出轨迹与理论输出轨迹差异最小的系数τ_steering和τ_velocity。

3.根据权利要求2所述的液压转向阿克曼型机器人的自动回充控制方法，其特征在于：所述步骤(1)中，构建机器人系统动态模型具体为：

机器人的阿克曼底盘在里程计坐标系中的运动描述如下：

其中，L_wheelbase表示机器人的轴距，即后轮到前轮的距离；

根据步骤(1)得到：

定义机器人系统状态变量为x_state＝[x,y,θ,δ,v]，机器人系统的控制量为u＝[v_control,δ_control]，则将其离散化为N时间段之后得到：

x_state(i+1)＝H[x_state(i),u(i)]

其中，x_state(i)表示i时刻的机器人系统状态，x_state(i+1)表示i+1时刻的机器人系统状态；u(i)表示i时刻机器人系统的控制量；i∈[1,2,...,N]；N表示总的预测控制总时间步长。

4.根据权利要求3所述的液压转向阿克曼型机器人的自动回充控制方法，其特征在于：所述步骤(2)中，根据回充任务要求计算得到期望机器人系统状态具体为：

假设整个回充过程在里程计坐标系下进行，充电桩位置和姿态由机器人感知系统计算给出，由此计算得到当前充电桩在里程计坐标系下的位姿^odomP_dock如下：

^odomP_dock＝[x_dock,y_dock,θ_dock]^T

其中，(x_dock,y_dock)表示充电桩在里程计坐标系下的坐标，θ_dock表示充电桩在里程计坐标系下的朝向；

根据充电桩在里程计坐标系下的位姿计算得到机器人的目标位姿：

^odomP_target＝F(^odomP_dock)＝[x_target,y_target,θ_target]

其中，F(^odomP_dock)为变换函数，根据机器人上充电口的位置将当前充电桩在里程计坐标系下的位姿变换为机器人的目标位姿；

根据自动回充任务要求得到充电桩在里程计坐标系下的目标位姿，据此得到机器人的目标位姿，进而得到期望的机器人系统状态：

x_target(i)＝[^odomP_target,0,0]^T。

5.根据权利要求4所述的液压转向阿克曼型机器人的自动回充控制方法，其特征在于：所述步骤(2)中，构建的模型预测控制的目标函数，并依据阿克曼底盘性能建立约束具体为：

构建模型预测控制的目标函数：

通过求取控制量序列[u(1),u(2),...u(N)]使得目标函数最小；

其中，控制量序列中各时刻控制量的初始值均为0；Q_N是轨迹末端权重矩阵，对角矩阵Q和R分别是系统状态轨迹权重矩阵和控制轨迹权重矩阵；

建立约束：

其中，X_min,X_max分别表示最小和最大允许的系统状态；ΔX_min,ΔX_max分别表示最小和最大允许的系统状态变化步长；U_min,U_max分别表示最小和最大允许的输入力矩；ΔU_min,ΔU_max分别表示最小和最大允许的直接力矩变化步长。

6.根据权利要求5所述的液压转向阿克曼型机器人的自动回充控制方法，其特征在于：所述步骤(3)具体为：

在某个控制周期i时刻，控制器求解步骤(2)的目标函数，得到i+1时刻的控制量序列u^＊＝[u(1),u(2),...u(N)]，则取u(1)下发到移动机器人底层控制器中，在该周期内采集系统状态信息，更新系统约束方程，将i时刻的控制量序列代入目标函数中求解得到第i+2时刻的控制量。

Images