[发明专利]一种分阶段的轮式机器人移动路径规划方法

权利要求书

1.一种分阶段的轮式机器人移动路径规划方法，其特征在于：用于实现在狭长的空间环境下任意全状态改变，将轮式机器人从一个已知的初始全状态转变到一个已知的目标全状态的这个过程划分为以下三个阶段，进行规划控制，所述轮式机器人的全状态包括轮式机器人的位置、速度和姿态，

1)速度压制阶段，该阶段在轮式机器人当前速度方向与当前目标位置方向不一致时，压制速度并调整方向，以尽快朝着目标方向移动；

2)直线段阶段，该阶段直线路径与小圆弧路径相切，以尽可能直线向着目标切点接近，同时控制到达的速度与姿态；

3)小圆弧段阶段，该阶段利用最大离心加速度限制圆弧路径的半径，使得轮式机器人沿着此圆弧形路径接近目标并且以期望的角度切入目标；

轮式机器人移动时不断修正进行移动路径规划，包括每历元根据当前时刻位置、速度和姿态，判断执行对应阶段的处理，直到最终到达目标状态；

不断修正进行移动路径规划，包括定义坐标系如下，

1)局部坐标系

以轮式机器人的中心P为坐标原点，轮式机器人的前方为Y轴正方向，右方为X轴正方向，在轮式机器人的运动过程中，该坐标系随着轮式机器人的移动而移动，用(X，Y)_Local标识；

2)全局坐标系

以空间中一点Q为坐标原点，水平方向为X轴，垂直方向为Y轴，在轮式机器人的运动过程中，该坐标系不发生平移和旋转，用(X，Y)_Global标识；

定义整体移动路径规划的路径如下，

1)A点为轮式机器人的瞬时位置，其质心坐标表示为：A：(X_A，Y_A)_Global，速度表示为(V_A，α_A)_Local，姿态表示为(β_A)_Global；

2)B点为目的地位置，其坐标表示为：B：(X_B，Y_B)_Global，速度表示为(V_B，α_B)_Local，姿态表示为(β_B)_Global；

3)C点为切点目标位置，其坐标表示为：C：(X_C，Y_C)_Global，速度表示为(V_C，α_C)_Local，姿态表示为(β_C)_Global；

并采用以下流程实现，

步骤S1，输入主体的起始状态以及目标状态，分别包括位置、速度、姿态三类信息；

对运动过程进行初步预估，并对特殊情况进行处理如下，

a)根据始、末位置连线的垂向空间大小限制小圆弧段最大半径R_Max，并且由最大离心加速度a_c约束，通过

计算得到当前环境下允许的小圆弧段最大速度V_{B_Max}，若V_B＞V_{B_Max}，则令V_B＝V_{B_Max}，此时由于空间受限，小车无法达到目标速度；

b)当目标速度为0时，将其设置为一个接近于0的较小正值，避免接近目的地的过程中运动的太慢，长时间无法抵达；

步骤S2，根据目标速度以及轮式机器人最大离心加速度的限制得到小圆弧半径的最小值，并根据轮式机器人当前的状态信息得到切点目标信息；实现如下，

a)求取小圆弧半径R，

V_B是目标速度大小；

a_c是轮式机器人最大离心加速度；

b)根据目的地的状态信息确定小圆弧圆心坐标，继而根据圆弧半径以及目标速度方向得到两个大小相同且相切的圆，圆心记为O1、O2，判断O1、O2中处于起始点和目标点中间的圆心位置；

c)计算切点的全状态信息如下，

切点C的速度大小V_c为：

V_c＝V_B

切点C的姿态β_c为：

β_c＝β_B

设切点C的位置为：C：(X_C，Y_C)_Global

为了简化解算上下切点的坐标，不考虑姿态，建立新坐标系；新坐标系以轮式机器人质心为原点，轮式机器人质心到圆弧圆心方向为X轴正方向，X轴逆时针旋转90度到达Y轴正方向，用(X，Y)_Temp标识；

记全局坐标系下圆心O：(X_O，Y_O)_Global，O到轮式机器人瞬时质心A的距离为L：

在新坐标系下，圆心点坐标O：(L，0)_Temp，圆弧半径为R，上方、下方切点坐标分别为：

新坐标系下坐标逆时针旋转θ后，平移(X_A，Y_A)_Global，转换到全局坐标系下：

其中

旋转矩阵

由以下公式将新坐标系下的坐标C：(X_{C_Temp}，Y_{C_Temp})_Temp转换到全局坐标系中：

新坐标系下目标速度方向为α_{B_Temp}

α_{B_Temp}＝α_{B_Global}+θ

为了使轮式机器人能够以小半圆弧行进，且达到目标要求的速度方向，切点C的位置判断依据如下：

if α_{B_Temp}＞0，则切点在圆心下方，否则，切点在圆心上方

由此得到唯一的切点C在新坐标系下的坐标C：(X_{C_Temp}，Y_{C_Temp})_Temp

全局坐标系下，唯一的切点C的坐标为：

X_C＝X_{C_Temp}cosθ+Y_{C_Temp}sinθ+X_A

Y_C＝Y_{C_Temp}cosθ-X_{C_Temp}sinθ+Y_A

步骤S3，若此时上一历元的状态为空，直接进入S3-1，若不为空，则通过相邻两个历元的状态信息对当前时刻轮式机器人所处的阶段进行判断，并做出相应处理；

当处于速度压制阶段，执行动作：通过上下两个历元位置变化计算出轮式机器人当前速度方向与切点目标位置方向之间的夹角，记为θ’，当角度不为零时，将cosθ’作为权值与轮式机器人运动的最大速度相乘，计算得到当前情况下的理想速度，以最大加速度进行加减速，使轮式机器人达到此理想速度；

当处于直线段阶段，执行动作：通过最终目标速度和当前时刻轮式机器人速度大小以及当前时刻与切点目标之间的距离对轮式机器人速度大小进行调控更新，实现加减速过程；

当处于小圆弧段阶段，执行动作：调整轮式机器人转向角度，并保持速度大小不变，走小圆弧段路径；

将轮式机器人上一历元位置标记为A1：(X_A1，Y_A1)_Global，当前历元位置标记为A2：(X_A2，Y_A2)_Global，切点位置为C：(X_C，Y_C)_Global，目的地为B：(X_B，Y_B)_Global；

全局坐标系下：

轮式机器人当前时刻的运动方向γ_A12为

切点位置方向γ_AC为

轮式机器人与切点目标之间的距离D_AC为：

相邻两历元轮式机器人到切点目标方向的夹角为：

步骤S4，记录轮式机器人当前历元的状态信息，与上一历元状态信息联合，判断目的地处是否处于相邻两历元中某位置，若是，则已经到达终点；否则，将当前历元状态信息传递到后续过程，并从步骤S2开始进行下一历元的处理。