[发明专利]基于自适应尺度约束优化的多非完整机器人车辆协同轨迹规划方法、装置及介质

权利要求书

1.一种基于自适应尺度约束优化的多非完整机器人协同轨迹规划方法，其特征在于，包括：

基于出行效率与乘客舒适度，构建机器人车辆的时间与能量成本代价函数模型；

基于机器人车辆的运动原理与行驶环境，起点与终点配置信息以及自身尺寸与行驶环境障碍物尺寸，依次构建机器人车辆的运动约束条件，边界约束条件以及避碰约束条件；

利用一阶显式龙格-库塔方法将在约束条件下中的时间与能量成本代价函数模型进行离散处理，并转化为求解机器人车辆与车辆、车辆与障碍物之间最小距离的NLP问题，然后通过自适应调整碰撞风险范围，迭代求解所述NLP问题，得到各机器人车辆的运动规划轨迹。

2.根据权利要求1所述的方法，其特征在于，所述机器人车辆的时间与能量成本代价函数模型如下：

其中，v_i(τ)、a_i(τ)表示第i辆机器人车辆τ时刻轮轴中点的速度、加速度，ω_i(τ)表示第i辆机器人车辆τ时刻的角速度，t_f表示机器人车辆轨迹终端时间，N_v表示机器人车辆数量，w表示加权参数，为经验设定值。

3.根据权利要求1所述的方法，其特征在于，所述机器人车辆的运动约束条件如下：

其中，(x_i(t)，y_i(t))表示第i辆机器人车辆t时刻后轮轴中点的坐标位置，v_i(t)、a_i(t)、jerk_i(t)表示第i辆机器人车辆t时刻轮轴中点的速度、加速度、加速度的导数，φ_i(t)、ω_i(t)、θ_i(t)表示第i辆机器人车辆t时刻的方向盘转角、角速度、定向角，L_Wi表示第i辆机器人车辆轴距，t_f表示终端时间，N_v表示机器人车辆数量；a_maxi、v_maxi、j_maxi、Ω_maxi、Φ_maxi分别表示第i辆机器人车辆的加速度最大值、速度最大值、加速度导数最大值、角速度最大值、方向盘转角最大值。

4.根据权利要求1所述的方法，其特征在于，所述边界约束条件是指机器人车辆在起点与终点时，轮轴中点的速度、加速度、加速度的导数、方向盘转角、角速度均为0，且机器人车辆在起点与终点时的轮轴中点的坐标与定向角为指定设置，具体如下：

其中，v_i(0)、a_i(0)、jerk_i(0)、φ_i(0)、ω_i(0)表示第i辆机器人车辆0时刻轮轴中点的速度、加速度、加速度的导数、方向盘转角、角速度，v_i(t_f)、a_i(t_f)、jerk_i(t_f)、φ_i(t_f)、ω_i(t_f)表示第i辆机器人车辆t_f时刻轮轴中点的速度、加速度、加速度的导数、方向盘转角、角速度，t_f表示机器人车辆轨迹终端时间，N_v表示机器人车辆数量。