考虑动力学约束和运行限制条件的避障重规划方法及系统技术方案

本发明专利技术属于智能体路径规划相关技术领域，其公开了一种考虑动力学约束和运行限制条件的避障重规划方法及系统，方法包括：构建考虑障碍物安全距离的智能体和障碍物交互模型；在智能体的前方设置随智能体运动的动态目标区域；采用路径规划方法得到初优化轨迹和初优化运动状态；将智能体抽象为智能体点质量模型，以智能体点质量模型为出发点做平行与轴与轴的两条直线，两条直线与初优化轨迹相交于两点，将两点中与目标点最近的点作为终目标点；构建预测控制模型；对预测控制模型进行求解并拟合获得连续的终优化运行状态和终优化轨迹。本申请可以对现有的路径规划进行优化，提高了避障成功率。避障成功率。避障成功率。

【技术实现步骤摘要】
考虑动力学约束和运行限制条件的避障重规划方法及系统


[0001]本专利技术属于机器人运动避障相关
，更具体地，涉及一种考虑动力学约束和运行限制条件的避障重规划方法及系统。

技术介绍

[0002]避障规划方法涉及对于实现智能体的自主导航运动具有十分重要的意义。目前的避障规划方法通常针对特定的全局路径地图进行目标性的运动涉及，要设计优秀的局部避障方法，不仅需要考虑障碍物检测和最优路径规划，还需要考虑智能体的运动控制性能和鲁棒性。同时，为确保智能体能够在复杂环境中稳定行动并完成任务目标，还需要考虑智能体的动力学约束和运动条件限制，常用的避障规划方法有人工势场法(APF)、动态窗口法(DWA)和向量场直方图法(VFH)等，均在局部避障规划方法有优异表现。但现有的避障规划方法在具体场景应用时仍然存在着一些问题：1)算法设计仅考虑图层面的运动规划，未针对实际的智能体动力学约束进行对应的轨迹更新；2)针对如障碍物分布较多的开放场景，往往智能体的运动会受到许多运动条件限制。因此，新的考虑动力学约束和运动限制条件的局部避障规划策略研究很有意义。

技术实现思路

[0003]针对现有技术的以上缺陷或改进需求，本专利技术提供了一种考虑动力学约束和运行限制条件的避障重规划方法及系统，可以对现有的路径规划进行优化，提高了避障成功率。
[0004]为实现上述目的，按照本专利技术的一个方面，提供了一种考虑动力学约束和运动限制条件的避障重规划方法，所述方法包括：S1：构建考虑障碍物安全距离的智能体和障碍物交互模型；S2：在智能体的前方设置随智能体运动的动态目标区域；S3：采用路径规划方法通过在所述动态目标区域中不断选择目标点对智能体的预设轨迹进行优化得到初优化轨迹和初优化运动状态；S4：将所述智能体抽象为智能体点质量模型，以智能体点质量模型为出发点做平行与X轴与Y轴的两条直线，两条直线与所述初优化轨迹相交于两点，将两点中与所述目标点最近的点作为终目标点；S5：采用待求终运动状态、初优化运动状态、控制输入函数以及惩罚函数构建预测控制模型；S6：对所述预测控制模型进行变形后求解获得离散的终优化运行状态和终优化轨迹；S7：采用曲线多项式拟合获得连续的终优化运行状态和终优化轨迹。
[0005]优选的，步骤S1中，智能体和障碍物交互模型为：
[0006][0007]其中，为避碰条件，当时，条件满足，智能体可以安全运行，x和y分别表示智能体的位置坐标，和分别代表障碍物中心的坐标，d
s
为雷达扫描范围，d
ero
为考虑安全距离的障碍物膨胀半径，d
ero
＝d
ero1
+
d
ero2
，d
ero2
为预设安全距离，d
ero1
为障碍物膨胀半径。
[0008]优选的，所述障碍物膨胀半径的公式为：
[0009][0010]其中，r
rotation
为智能体本体回转半径，为智能体运行过程中可能出现的干涉范围，为其中的最大值，为智能体干涉范围边界到障碍物的距离的最大值。
[0011]优选的，步骤S2中，所述动态目标区域由动态目标点区域中心向两侧扩展形成，其中，所述动态目标点区域中心的前进策略设计为：
[0012][0013]其中，为t时刻的动态目标点区域，Γ为动态目标点区域移动的调整因子，V
l
为沿着原始参考轨迹向前移动的速度，T为采样周期，d为与障碍物相关的动态目标点区域的初始距离，τ为偏航校正参数，υ为障碍物大小校正参数，τ，υ∈R
+
，ψ(t)为每个时刻智能体头部朝向和动态目标点在原始轨迹上的切线方向之间的角度，为感知障碍物的传感器角度。
[0014]优选的，所述动态目标区域的表达式为：
[0015][0016]其中，和分别表示动态目标点区域的上界和下界，κ∈R
+
，D(x)是动态目标点区域。
[0017]优选的，步骤S4中，所述智能体点质量模型为：
[0018][0019]|μ(t)|＜ug
[0020]其中，ξ(t)为t时刻智能体的运动状态，其中，ξ(t)为t时刻智能体的运动状态，为智能体在x轴方向上的速度，为智能体在y轴方向上的速度，θ为航向角，Y为智能体在t时刻位置的纵坐标，X为智能体在t时刻位置的横坐标，为ξ(t)的导数，μ(t)为t时刻的输入，g为重力加速度，u为摩擦系数。
[0021]优选的，步骤S5中，预测控制模型为：
[0022][0023]s.t.U
min
≤U
t
≤U
max
[0024]其中，N
p
为预测时域，U
i
为控制输入点集合，J
obs，i
为采用时刻i的惩罚函数，Q为权重系数，U
min
和U
max
分别为其最大值和最小值，ξ
ref
(t+i|t)为初优化运动状态参数，U
t
为控制输入序列，为初优化运动状态中智能体在y轴方向上的速度，为初优化运动状态中智能体在x轴方向上的速度，θ
ref
为初优化运动状态中智能体的航向角，Y
ref
为初优化运动状态中智能体位置的纵坐标，X
ref
为初优化运动状态中智能体位置的横坐标；
[0025]所述惩罚函数为：
[0026][0027]其中，S
obs
为权重系数，V
l
为智能体底盘的纵向速度大小，(x
i
，y
i
)为智能体坐标系下障碍物点云的位置坐标，(x0，y0)为智能体质心坐标，为小值正数。
[0028]优选的，步骤S6中对所述预测控制模型进行变形获得标准二次型规划形式，采用MATLAB自带的规划求解器进行求解。
[0029]优选的，所述障碍物需要进行如下处理：根据激光雷达和红外传感器提供的障碍物点云信息；将距离小于预设距离的障碍物点云采用聚类算法合并为大型障碍物；对于距离大于预设距离的障碍物点云视为离散的小障碍物。
[0030]优选的，步骤S3中路径规划方法为DWA或VFH或APF。
[0031]本申请另一方面提供了一种考虑动力学约束和运动限制条件的局部重规划系统，所述系统包括：第一构建模块：用于构建考虑障碍物安全距离的智能体和障碍物交互模型；设置模块：用于在智能体的前方设置随智能体运动的动态目标区域；初步规划模块：用于采用路径规划方法通过在所述动态目标区域中不断选择目标点对智能体的预设轨迹进行优化得到初优化轨迹和初优化运动状态；目标点获取模块：用于将所述智能体抽象为智能体点质量模型，以智能体点质量模型为出发点做平行与轴与轴的两条直线，两条直线与所述初优化轨迹相交于两点，将两点中与所述目标点最近的点作为终目标点；第二构建模块：用于采用待求终运动状态、初优化运动状态本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种考虑动力学约束和运动限制条件的避障重规划方法，其特征在于，所述方法包括：S1：构建考虑障碍物安全距离的智能体和障碍物交互模型；S2：在智能体的前方设置随智能体运动的动态目标区域；S3：采用路径规划方法通过在所述动态目标区域中不断选择目标点对智能体的预设轨迹进行优化得到初优化轨迹和初优化运动状态；S4：将所述智能体抽象为智能体点质量模型，以智能体点质量模型为出发点做平行与X轴与Y轴的两条直线，两条直线与所述初优化轨迹相交于两点，将两点中与所述目标点最近的点作为终目标点；S5：采用待求终运动状态、初优化运动状态、控制输入函数以及惩罚函数构建预测控制模型；S6：对所述预测控制模型进行变形后求解获得离散的终优化运行状态和终优化轨迹；S7：采用曲线多项式拟合获得连续的终优化运行状态和终优化轨迹。2.根据权利要求1所述的方法，其特征在于，步骤S1中，智能体和障碍物交互模型为：其中，为避碰条件，当时，条件满足，智能体可以安全运行，x和y分别表示智能体的位置坐标，和分别代表障碍物中心的坐标，d
s
为雷达扫描范围，d
ero
为考虑安全距离的障碍物膨胀半径，d
ero
＝d
ero1
+d
ero2
，d
ero2
为预设安全距离，d
ero1
为障碍物膨胀半径，所述障碍物膨胀半径的公式为：其中，r
rotation
为智能体本体回转半径，为智能体运行过程中可能出现的干涉范围，为其中的最大值，为智能体干涉范围边界到障碍物的距离的最大值。3.根据权利要求1所述的方法，其特征在于，步骤S2中，所述动态目标区域由动态目标点区域中心向两侧扩展形成，其中，所述动态目标点区域中心的前进策略设计为：其中，为t时刻的动态目标点区域，Γ为动态目标点区域移动的调整因子，V
l
为沿着原始参考轨迹向前移动的速度，T为采样周期，d为与障碍物相关的动态目标点区域的初始距离，τ为偏航校正参数，υ为障碍物大小校正参数，ψ(t)为每个时刻
智能体头部朝向和动态目标点在原始轨迹上的切线方向之间的角度，为感知障碍物的传感器角度。4.根据权利要求3所述的方法，其特征在于，所述动态目标区域的表达式为：其中，和分别表示动态目标点区域的上界和下界，κ∈R
+
，D(x)是动态目标点区域。5.根据权利要求1所述的方法，其特征在于，步骤S4中，所述智能体点质量模型为：|μ(t)|＜ug其中，ξ(t)为t时刻智能体的运动状态，其中，ξ(t)为t时刻智能体的运动状态，为智能体在x轴方向上的速度，为智能体在y轴方向上的速度，θ为航向角，Y为智能体在t时刻位置的纵坐标，X为智能体在t时刻位置的横坐标，为ξ(t)的导数，μ(t)为t时刻的输入，...

【专利技术属性】
技术研发人员：谢远龙，张鸿洋，严一鸣，王书亭，李虎，张赛，
申请(专利权)人：华中科技大学，
类型：发明
国别省市：