CN114355929B - 轨迹柔顺切换的移动机器人自适应避障控制方法 - Google Patents

Abstract

本发明属于机器人控制与规划相关技术领域，其公开了一种轨迹柔顺切换的移动机器人自适应避障控制方法，方法包括：构建运动学模型和运动空间状态方程；构建自适应抗扰终端滑模面；设计等效控制律，并设计趋近律；以趋近律和等效控制律为参数构建前轮输入转角和后轮输入转角；以移动机器人的中心为圆心设置子目标点区域，并设置移动机器人与子目标区域移动速度的匹配关系；采用路径规划算法对机器人的路径进行规划；构建子目标点区域中目标点与移动机器人中心点的目标代价函数，以目标代价函数最优为目标实现对移动机器人运动轨迹的修正。本申请采用自适应分阶段滑模控制，既保证了收敛速度，又提升了稳定性，便于轨迹改变时平滑切换。

Description

轨迹柔顺切换的移动机器人自适应避障控制方法

技术领域

本发明属于机器人控制与规划相关技术领域，更具体地，涉及一种轨迹柔顺切换的移动机器人自适应避障控制方法。

背景技术

移动机器人的轨迹跟踪控制方法设计对于移动机器人的高效运输和作业具有十分重要的意义，目前的轨迹跟踪控制通常针对特定的轨迹进行有目标性的轨迹误差控制，在移动机器人应用于动态障碍物较多的跟踪控制的过程中不考虑轨迹的改变，或是重规划的轨迹与原始轨迹的切换部分存在大幅度的轨迹角度和速度突变，从而造成切换过程的系统振荡，影响移动机器人的控制性能。常用的机器人控制方法有PID控制、模型预测控制和滑模控制等，其中，滑模控制由于其优秀的鲁棒性和易实施性在轨迹跟踪控制器的设计中得到了广泛的应用，但现有的滑模控制(例如，中国专利CN112327620)仍然存在以下问题：1)考虑控制系统收敛速度时仅考虑收敛到滑模面的时间，对到达滑模面系统的稳定性和误差收敛时间缺少关注；2)传统滑模控制在距滑模面较远和趋近滑模面时收敛速度保持一致，过大的收敛速度会导致趋近滑模面时系统发生振荡，影响移动机器人的运动精度，过小的收敛速度会导致系统趋近滑模面的速度过慢，降低移动机器人的自适应速度。

发明内容

针对现有技术的以上缺陷或改进需求，本发明提供了一种轨迹柔顺切换的移动机器人自适应避障控制方法，采用自适应分阶段滑模控制，既保证了收敛速度，又提升了稳定性，便于轨迹改变时平滑切换。

为实现上述目的，按照本发明的一个方面，提供了一种轨迹柔顺切换的移动机器人自适应避障控制方法，所述方法包括：S1：构建以航向角误差值为参数的运动学模型和运动空间状态方程；S2：基于所述运动空间状态方程构建线性滑模面和积分滑模面，并将所述线性滑模面和积分滑模面叠加获得自适应抗扰终端滑模面Θ：Θ＝Θ₁+Θ₂，其中，Θ₁为线性滑模面，Θ₂为积分滑模面，χ₁，χ₂，χ₃，χ₄为预设的控制增益，均大于0，β₁,β₂为预设的误差系数，β₁>1，1>β₂>0，θ_e为所述航向角误差值；S3：将所述自适应抗扰终端滑模面求导设计等效控制律，并基于所述自适应抗扰终端滑模面设计趋近律；S4：以所述趋近律和等效控制律为参数构建运动学模型所需的前轮输入转角和后轮输入转角；S5：待移动机器人需要避障时，以所述移动机器人的中心为圆心设置子目标点区域，并设置移动机器人与子目标区域移动速度的匹配关系；S6：采用路径规划算法对机器人的路径进行规划；S7：构建子目标点区域中目标点与移动机器人中心点的目标代价函数，以所述目标代价函数最优为目标实现对移动机器人运动轨迹的修正。

优选地，步骤S3中所述将所述自适应抗扰终端滑模面求导设计等效控制律具体为对所述自适应抗扰终端滑模面求导并将其导函数赋值为0时获得所述等效控制律。

优选地，所述等效控制律Con为：

其中，ρ＝V_l/L，V_l是移动机器人的轮系速度值，L为前轮中心和后轮中心轴向距离，θ_r为航向角的参考值，表示对θ_r进行一次求导操作，/>表示对θ_e进行二次求导操作；

所述趋近律W为：

其中，q₁,q₂满足以下条件：

q₁＝m₀+m₁tanh(Θ^r)-m₂tanh(λΘ²)

其中，

各参数满足r>0,λ>0,0<m₂<m₀<1,m₁>1,m₀+m₁-m₂>1。

h₁,h₂,h₃满足h₂>0,h₃>0，q为预设滑模面幂次参数，r为预设参数。

优选地，所述前轮转角δ_f和后轮转角δ_r满足如下关系式：

其中，Con为所述等效控制律，W为所述趋近律。

优选地，步骤S6中所述路径规划算法为向量直方图算法，所述向量直方图算法函数为：

m_i,j＝C_i,j ²(a-bd_i,j)

k_f＝k_n+s_max

其中，β'_i,j为世界坐标下特定障碍物到移动机器人的夹角，x_yj为障碍物单元格y坐标，x_y0为移动机器人当前y坐标，x_xj为障碍物单元格x坐标，x_x0为移动机器人当前x坐标，m_i,j为单元格(i,j)处的障碍矢量大小，C_i,j为获得单元格的障碍物确定值，a和b为障碍物密度参数，d_i,j为单元格到机器人的距离，k为障碍物角度对应的扇形区域编号，α'为设定的角度分辨率，H_k为极坐标障碍物密度函数，H_k'为平滑处理后的极坐标障碍物密度函数，l为平滑处理系数，k_f为前进区域的远边界，k_n为离目标点方向最近的小于密度临界值的角度方向，s_max为设定的前进区域最大转角，θ'为移动机器人当前的参考前进方向。

优选地，步骤S6还包括采用所述移动机器人当前的参考前进方向θ'对所述移动机器人的运动空间状态方程进行修正。

优选地，所述子目标点区域中心的前进控制方程为：

Γ＝τ×cosψ(t)

d＝υ×sinζ

其中，τ,υ∈R⁺,G为子目标点区域前进的动态调节因子，ψ为每一时刻移动机器人的航向与原始运动目标点在原始轨迹上的速度切线方向的夹角，t为t时刻，d为与障碍物相关的子目标点区域初始增量，ζ为检测到障碍物时雷达检测区域与障碍物重叠部分对应在区域内的圆弧角，分别为子目标点区域的上下界，其中κ∈R⁺，D(x)为子目标点区域。

优选地，所述目标代价函数为：

COST(i)＝α·SwitchAngel(x_i,x_o)+β·length(x_i,x_o)

其中，α,β为调参系数，Switch Angel为回归原始轨迹时机器人的速度方向和轨迹切线方向的夹角，length为绕障重规划路径长度，x_i为子目标点区域中第i个目标点，x_o为移动机器人的中心。

优选地，步骤S5中采用如下方式判断移动机器人是否需要避障：建立环境感知交互模型区域，包括雷达检测区域、不可行区域和可行区域，若所述雷达检测区域和不可行区域的交集不为空集则启动避障规划。

优选地，所述移动机器人的运行学模型为：

平移模式的运动学模型为：

普通阿克曼模式为：

变阿克曼模式为：

其中，x，y是移动机器人在世界坐标系下的坐标值，θ为移动机器人的航向角，V_l为移动机器人的轮系速度值，δ_f,δ_r分别为移动机器人的前轮转角和后轮转角，L为前轮中心和后轮中心轴向距离；

移动机器人的运动空间状态方程为：

其中，ρ＝V_l/L，θ_e航向角的误差值，θ_r为航向角的参考值，γ为考虑系统振荡和建模不确定性所产生的等效误差。

总体而言，通过本发明所构思的以上技术方案与现有技术相比，本发明提供的轨迹柔顺切换的移动机器人自适应避障控制方法具有如下有益效果：

1.采用自适应分阶段终端滑模面就那些轨迹跟踪控制，线性滑模面保证在距离滑模面较远时具有较大的收敛速度，积分滑模面保证在距离滑模面较小时具有较小的收敛速度，既保证了移动机器人对外界误差的快速自适应能力，又降低了因收敛速度过快导致的滑模面不稳定，提高了机器人系统的跟踪精度和鲁棒性。

2.针对移动机器人避障轨迹重规划提出了子目标点追踪策略，动态匹配子目标点区域和移动机器人，可得到每一时刻重规划轨迹的最优解，保证重规划轨迹和原始轨迹柔顺切换，降低移动机器人的轨迹切换振荡，实现了轨迹跟踪过程的高效、平滑避障。

3.本申请通过设置子目标点区域与移动机器人的目标代价函数可以实现机器人避障后平滑切换至原始路径。

附图说明

图1是轨迹柔顺切换的移动机器人自适应避障控制方法的步骤图；

图2是轨迹柔顺切换的移动机器人自适应避障控制方法的流程图；

图3是移动机器的运动模式，其中，(a)为变阿克曼模式，(b)为阿克曼模式，(c)为平移模式；

图4是移动机器人避障阶段的流程图；

图5是轨迹柔顺切换的移动机器人自适应避障的运动示意图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。

本发明提供了一种轨迹柔顺切换的移动机器人自适应避障控制方法，如图1和图2所示，具体如下步骤S1～S7。

S1：构建以航向角误差值为参数的运动学模型和运动空间状态方程。

移动机器人的运动模式有平移模式、阿克曼模式和变阿克曼模式(如图3所示)，以上模式的运动学模型如下：

平移模式为：

普通阿克曼模式为：

变阿克曼模式为：

其中，x，y是移动机器人在世界坐标系下的坐标值，θ为移动机器人的航向角，V_l为移动机器人的轮系速度值，δ_f,δ_r分别为移动机器人的前轮转角和后轮转角，L为前轮中心和后轮中心轴向距离。

移动机器人的运动空间状态方程为：

其中，ρ＝V_l/L，θ_e航向角的误差值，θ_r为航向角的参考值，γ为考虑系统振荡和建模不确定性所产生的等效误差。

移动机器人在保持轮系速度值不变的情况下，可以通过改变运动模式和控制前后轮转角实现对轨迹的跟踪和绕障切换，因此，柔顺切换的重点在于对输入参数δ_f,δ_r的优化。

S2：基于所述运动空间状态方程构建线性滑模面和积分滑模面，并将所述线性滑模面和积分滑模面叠加获得自适应抗扰终端滑模面Θ：

Θ＝Θ₁+Θ₂

其中，Θ₁为线性滑模面，Θ₂为积分滑模面， χ₁，χ₂，χ₃，χ₄为预设的控制增益，均大于0，β₁，β₂为预设的误差系数，β₁＞1，1＞β₂＞0，θ_e为所述航向角误差值。

S3：将所述自适应抗扰终端滑模面求导设计等效控制律，并基于所述自适应抗扰终端滑模面设计趋近律。

当系统运动到滑模面上时，滑模面的导数为0，此时得到滑模面的等效控制律Con：

其中，ρ＝V_l/L，V_l是移动机器人的轮系速度值，L为前轮中心和后轮中心轴向距离，θ_r为航向角的参考值，表示对θ_r进行一次求导操作，/>表示对θ_e进行二次求导操作；

所述趋近律Ω为：

其中，q₁，q₂满足以下条件：

q₁＝m₀+m₁tanh(Θ^r)-m₂tanh(λΘ²)

其中，

各参数满足r＞0，λ＞0，0＜m₂＜m₀＜1，m₁＞1，m₀+m₁-m₂＞1。

h₁，h₂，h₃满足h₂＞0，h₃＞0，q为预设滑模面幂次参数，r为预设参数。

S4：以所述趋近律和等效控制律为参数构建运动学模型所需的前轮输入转角和后轮输入转角。

所述前轮输入转角δ_f和后轮输入转角δ_r满足如下关系式：

其中，Con为所述等效控制律，Ω为所述趋近律。

构造李雅普诺夫函数V＝0.5Θ²，可知满足V≥0，将前轮输入转角δ_f和后轮输入转角δ_r代入运动空间状态方程通过计算可得：

因此可以得到有如下形式：

由上式可以看出，由于 满足

由李雅普诺夫稳定性理论可知，提出的前后轮转角输入能够保证系统最终稳定于滑模面，即系统渐进稳定。

S5：待移动机器人需要避障时，以所述移动机器人的中心为圆心设置子目标点区域，并设置移动机器人与子目标区域移动速度的匹配关系。

首先需要进行避障判定，如图4和图5所示，具体如下：

建立移动机器人的环境感知交互模型区域Z，其中包括雷达检测区域Z_de、不可行区域Z_nof(不仅包括场景中的静态障碍物和边界区域，还包括环境中未知的动态障碍物)和可行区域Z_if。

Z_de(t)＝{x_s|||x_s-x|≤d_s}

Z_nof(t)＝{x_s||g(x_s)＝1}

Z_if(t)＝{x_s||g(x_s)＝0}

其中，g(x_s)为地图二值化函数，值为1则说明存在障碍物或者边界阻碍，值为0则为移动机器人可行区域，固移动机器人的环境感知交互区域为Z＝Z_de∪Z_nof∪Z_if。

当移动机器人的雷达检测区域和不可行区域发生重叠时，即系统即进入避障规划阶段，避障启动时以所述移动机器人的中心为圆心设计长轴在原始轨迹的椭圆，所述椭圆区域即为子目标点区域。

然后还需要设置移动机器人与子目标区域移动速度的比例关系，使得机器人与子目标区域按照一定的比例进行前进，其中，所述子目标点区域中心的前进控制方程为：

Г＝τ·cosψ(t)

d＝υ·sinζ

其中，τ，υ∈R⁺，Г为子目标点区域前进的动态调节因子，ψ为每一时刻移动机器人的航向与原始运动目标点在原始轨迹上的速度切线方向的夹角，t为t时刻，d为与障碍物相关的子目标点区域初始增量，ζ为检测到障碍物时雷达检测区域与障碍物重叠部分对应在区域内的圆弧角，分别为子目标点区域的上下界，其中κ∈R⁺，D(x)为子目标点区域。

S6：采用路径规划算法对机器人的路径进行规划。

本实施例中路径规划算法优选为向量直方图算法，所述向量直方图算法函数为：

m_i，j＝C_i，j ²(a-bd_i，j)

k_f＝k_n+s_max

其中，β′_i，j为世界坐标下特定障碍物到移动机器人的夹角，x_yj为障碍物单元格y坐标，x_y0为移动机器人当前y坐标，x_xj为障碍物单元格x坐标，x_x0为移动机器人当前x坐标，m_i，j为单元格(i，j)处的障碍矢量大小，C_i，j为获得单元格的障碍物确定值，a和b为障碍物密度参数，d_i，j为单元格到机器人的距离，k为障碍物角度对应的扇形区域编号，α′为设定的角度分辨率，H_k为极坐标障碍物密度函数，H_k′为平滑处理后的极坐标障碍物密度函数，l为平滑处理系数，k_f为前进区域的远边界，k_n为离目标点方向最近的小于密度临界值的角度方向，s_max为设定的前进区域最大转角，θ′为移动机器人当前的参考前进方向。

采用所述移动机器人当前的参考前进方向θ′对所述移动机器人的运动空间状态方程进行修正。具体为采用参考前进方向θ′对航向角的参考值θ_r进行修正。

S7：构建子目标点区域中目标点与移动机器人中心点的目标代价函数，以所述目标代价函数最优为目标实现对移动机器人运动轨迹的修正。

所述目标代价函数为：

COST(i)＝α·SwitchAngel(x_i，x_o)+β·length(x_i，x_o)

其中，α，β为调参系数，Switch Angel为回归原始轨迹时机器人的速度方向和轨迹切线方向的夹角，length为绕障重规划路径长度，x_i为子目标点区域中第i个目标点，x_o为移动机器人的中心。选取子目标点区域中目标代价函数最小的点作为区域内的子目标点，即实现了任意时刻重规划轨迹和原始轨迹的最优切换。

以上步骤S5～S7的伪代码为：

Spsubcenter：设计的动态子目标点区域中心；

Subgoalselect(Γ，V_l，d)：子目标点区域追踪策略函数；

Spsubregion：子目标点区域；

Subgoalerode(Spsubcenter，κ)：子目标点区域扩展函数；

Spsub：选取的用于轨迹重规划的子目标点；

randomselect(Spsubregion，c)：子目标点区域内规划目标点的选取函数；

VFH(x_o，s_max，α′，Spsub)：向量场直方图算法函数；

POD，pod：重规划路径集合和其中的子路径；

calcauco(POD1，α，β)：重规划路径加权目标函数；

COST，trajectory：重规划路径目标函数权值和对应路径。

本领域的技术人员容易理解，以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims (8)

1.一种轨迹柔顺切换的移动机器人自适应避障控制方法，其特征在于，所述方法包括：

S1：构建以航向角误差值为参数的运动学模型和运动空间状态方程；

运行学模型为：

平移模式的运动学模型为：

普通阿克曼模式为：

变阿克曼模式为：

其中，x，y是移动机器人在世界坐标系下的坐标值，θ为移动机器人的航向角，V_l为移动机器人的轮系速度值，δ_f,δ_r分别为移动机器人的前轮转角和后轮转角，L为前轮中心和后轮中心轴向距离；

移动机器人的运动空间状态方程为：

其中，ρ＝V_l/L，θ_e航向角的误差值，θ_r为航向角的参考值，γ为考虑系统振荡和建模不确定性所产生的等效误差；

S2：基于所述运动空间状态方程构建线性滑模面和积分滑模面，并将所述线性滑模面和积分滑模面叠加获得自适应抗扰终端滑模面Q：

Q＝Q₁+Q₂

其中，Q₁为线性滑模面，Q₂为积分滑模面， χ₁，χ₂，χ₃，χ₄为预设的控制增益，均大于0，β₁,β₂为预设的误差系数，β₁>1，1>β₂>0，θ_e为所述航向角误差值；

S3：将所述自适应抗扰终端滑模面求导设计等效控制律，并基于所述自适应抗扰终端滑模面设计趋近律；

所述等效控制律Con为：

其中，ρ＝V_l/L，V_l是移动机器人的轮系速度值，L为前轮中心和后轮中心轴向距离，θ_r为航向角的参考值，表示对θ_r进行一次求导操作，/>表示对θ_e进行二次求导操作；

S4：以所述趋近律和等效控制律为参数构建运动学模型所需的前轮输入转角和后轮输入转角；

S5：待移动机器人需要避障时，以所述移动机器人的中心为圆心设置子目标点区域，并设置移动机器人与子目标区域移动速度的匹配关系；

所述设置移动机器人与子目标区域移动速度的匹配关系也即移动机器人与子目标区域移动速度的比例关系，使得机器人与子目标区域按照一定的比例进行前进，其中，所述子目标点区域中心的前进控制方程为：

Γ＝τ·cosψ(t)

d＝v·sinζ

其中，τ，υ∈R⁺，Γ为子目标点区域前进的动态调节因子，ψ为每一时刻移动机器人的航向与原始运动目标点在原始轨迹上的速度切线方向的夹角，t为t时刻，d为与障碍物相关的子目标点区域初始增量，ζ为检测到障碍物时雷达检测区域与障碍物重叠部分对应在区域内的圆弧角，分别为子目标点区域的上下界，其中κ∈R⁺，D(x)为子目标点区域；

S6：采用路径规划算法对机器人的路径进行规划；

S7：构建子目标点区域中目标点与移动机器人中心点的目标代价函数，以所述目标代价函数最优为目标实现对移动机器人运动轨迹的修正。

2.根据权利要求1所述的方法，其特征在于，步骤S3中所述将所述自适应抗扰终端滑模面求导设计等效控制律具体为对所述自适应抗扰终端滑模面Θ求导并将其导函数赋值为0时获得所述等效控制律。

3.根据权利要求1所述的方法，其特征在于，

所述趋近律Ω为：

其中，q₁，q₂满足以下条件：

q₁＝m₀+m₁tanh(Θ^r)-m₂tanh(λΘ²)

其中，

各参数满足r>0,λ>0,0<m₂<m₀<1,m₁>1,m₀+m₁-m₂>1，

h₁,h₂,h₃满足h₂>0,h₃>0，q为预设滑模面幂次参数，r为预设参数。

4.根据权利要求1或3所述的方法，其特征在于，前轮转角δ_f和后轮转角δ_r满足如下关系式：

其中，Con为所述等效控制律，W为所述趋近律。

5.根据权利要求1所述的方法，其特征在于，步骤S6中所述路径规划算法为向量直方图算法，所述向量直方图算法函数为：

m_i,j＝C_i,j ²(a-bd_i,j)

k_f＝k_n+s_max

其中，β'_i,j为世界坐标下特定障碍物到移动机器人的夹角，x_yj为障碍物单元格y坐标，x_y0为移动机器人当前y坐标，x_xj为障碍物单元格x坐标，x_x0为移动机器人当前x坐标，m_i,j为单元格(i,j)处的障碍矢量大小，C_i,j为获得单元格的障碍物确定值，a和b为障碍物密度参数，d_i,j为单元格到机器人的距离，k为障碍物角度对应的扇形区域编号，α'为设定的角度分辨率，H_k为极坐标障碍物密度函数，H_k'为平滑处理后的极坐标障碍物密度函数，l为平滑处理系数，k_f为前进区域的远边界，k_n为离目标点方向最近的小于密度临界值的角度方向，s_max为设定的前进区域最大转角，θ'为移动机器人当前的参考前进方向。

6.根据权利要求5所述的方法，其特征在于，步骤S6还包括采用所述移动机器人当前的参考前进方向θ'对所述移动机器人的运动空间状态方程进行修正。

7.根据权利要求1或5所述的方法，其特征在于，所述目标代价函数为：

COST(i)＝α×SwitchAngel(x_i,x_o)+β×length(x_i,x_o)

其中，α,β为调参系数，Switch Angel为回归原始轨迹时机器人的速度方向和轨迹切线方向的夹角，length为绕障重规划路径长度，x_i为子目标点区域中第i个目标点，x_o为移动机器人的中心。

8.根据权利要求1所述的方法，其特征在于，步骤S5中采用如下方式判断移动机器人是否需要避障：

建立环境感知交互模型区域，包括雷达检测区域、不可行区域和可行区域，若所述雷达检测区域和不可行区域的交集不为空集则启动避障规划。