CN108594815B - 一种分阶段的轮式机器人移动路径规划方法 - Google Patents

Abstract

本发明公开一种分阶段的轮式机器人移动路径规划方法，将轮式机器人从一个已知的初始全状态转变到一个已知的目标全状态的这个过程划分为三个阶段，进行规划控制，速度压制阶段在轮式机器人当前速度方向与当前目标位置方向不一致时，压制速度并调整方向，以尽快朝着目标方向移动；直线段阶段中，直线路径与小圆弧路径相切，以尽可能直线向着目标切点接近，同时控制到达的速度与姿态；小圆弧段阶段中利用最大离心加速度限制圆弧路径的半径，使得轮式机器人沿着此圆弧形路径接近目标并且以期望的角度切入目标。本发明不仅能够实现轮式机器人全状态的改变，并且对周遭环境没有空间大小方面的要求，具有较强的普适性和稳定性。

Description

一种分阶段的轮式机器人移动路径规划方法

技术领域

本发明属于机器人运动控制技术领域，具体涉及一种分阶段的轮式机器人移动路径规划方法。

背景技术

随着工业自动化的不断发展，诸如智能小车、移动机器人等轮式机器人在日常生活中的应用也越来越广泛，引导运动主体的移动，已经不仅仅是位置的移动，更包括姿态和速度的改变。高效且稳定的移动方式是后续上层工作的基础，如何能够便捷并且流畅的完成移动过程一直以来都是争相探讨的话题。

为了解决初、末全状态改变中，末速度方向与初、末位置矢量方向不一致的问题，通常情况下是通过一段从起始点到目标点，并且与始末速度方向相切的大圆弧路径完成的。这种方式只有一个过程，相对简单，但是，沿着大圆弧段路径行进，所需的空间太大，对于环境空间有一定的要求，条形空间或较窄的环境下无法使用，正是由于这一弊端，迫切的需要一种更具有普适性的移动方法，本文就是基于这一点，提出了新的移动路径规划方法，可以很好的解决以上问题。

另一方面，本方法能够同步改变姿态、速度、位置，与目前广为采用的将姿态转变、位置转变、速度转变独立依次完成的方法相区别，本方法更加流畅快速。

其次，移动系统的部件控制延迟会导致行进路径产生偏离。如，当轮式机器人本身就具有较大的速度，设置轮式机器人进行转向并且从初始位置到达设定值的这一过程是需要时间的，在这个过程中路径将产生一个较大的偏离，如果没有得到很好的处理，当偏差较大时，可能导致无法到达预设目标状态，甚至轮式机器人失控。本文就这一问题，提出了速度压制方案，可以有效的削弱转向渐变产生的偏离。

发明内容

为了解决现有技术存在的问题，在已知独立控制车轮方向和速度大小的轮式机器人可以实现位置、速度、姿态的同步改变，并且车轮不会侧向横移的前提下，本发明提供一种分阶段的轮式机器人移动路径规划方法，核心技术就是创新性的采用直线+小圆弧的移动路径，从而能够快速平稳的引导移动主体朝着预定的目的进行位置、速度、姿态改变，同时综合考虑了系统部件延迟，提高了各种情景下的适应性、稳定性。

本发明所采用的技术方案提供一种分阶段的轮式机器人移动路径规划方法，将轮式机器人从一个已知的初始全状态转变到一个已知的目标全状态的这个过程划分为以下三个阶段，进行规划控制，所述轮式机器人的全状态包括轮式机器人的位置、速度和姿态，

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

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

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

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

而且，不断修正进行移动路径规划，采用以下流程实现，

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

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

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

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

而且，判断阶段的实现如下，

1)当相邻两历元位置变化方向与切点目标位置方向相差大于预设的许可角度偏差阈值，且与目的地的距离大于小圆弧段最小半径时，标识第一阶段；

2)当相邻两历元位置变化方向与切点目标位置方向相差小于等于预设的许可角度偏差阈值，且与目的地的距离大于小圆弧段最小半径时，标识第二阶段；

3)切点目标位置位于相邻两历元某中间位置时，标识第三阶段。

本发明的优点为：

(1)此方法适用于所有轮式机器人，是一种通用性较强的方法。

(2)本发明中直线+小圆弧的方式，不仅能够很好的实现轮式机器人全状态的改变，并且对周遭环境不存在空间大小方面的要求，解决了常规几何关系解算中大圆弧模型无法适应较小空间的情况，该模型适用于各种情景，具有较强的普适性。

(3)考虑到了部件控制延迟，在应用中，当移动主体给定初始速度，由于设置转向角度是逐渐变化的过程，因此无法达到预期的直线效果，而会形成一段弧线路径，采用对速度进行压制的方式，削弱转角渐变产生的弧线效应，使系统更加可控。

(4)考虑到判断到达目的与否条件的合理性，采用了相邻时间节点对比的方式，相较于阈值判断，具有更好的稳定性。

(5)综合考虑了移动主体的离心加速度，最大加速度以及最大速度等物理特性，最大限度的保证了轮式机器人快速稳定的运动，更加高效平稳，在实际应用中具有很大的优势。

(6)每历元都对状态进行重新计算、判断，增加了移动系统的容错性、可靠性。

附图说明

图1为本发明实施例的全局坐标系，局部坐标系下轮式机器人姿态、速度示意图。

图2为本发明实施例的分阶段移动路径规划路径示意图。

图3为本发明实施例的计算全局坐标系下速度方向几何关系示意图。

图4为本发明实施例的确定圆弧圆心的几何关系示意图。

图5为本发明实施例的确定切点位置的几何关系示意图。

图6为本发明实施例的移动路径规划方案流程示意图。

具体实施方式

以下结合附图和实施例详细说明本发明的技术方案。

本发明将轮式机器人的位置、速度、姿态三者合称为轮式机器人的全状态，此轮式机器人可以同时独立调控速度大小和方向，本发明将轮式机器人从一个已知的初始全状态转变到一个已知的目标全状态的这个过程划分为三个阶段。为了解决初、末全状态改变中，末速度方向与初、末位置矢量方向不一致的问题，区别于传统做法中需要较宽的空间进行速度方向的切入，采用本方法可以实现在较为狭长的空间环境下任意全状态改变；另一方面，本方法能够同步改变姿态、速度、位置，与目前广为采用的将姿态转变、位置转变、速度转变独立依次完成的方法相区别，本方法更加流畅快速。

三个阶段如下：

1)速度压制阶段：考虑到行进速度与目标方向不一致的情况，压制速度并调整方向，以尽快朝着目标方向移动

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

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

第一阶段速度压制阶段，通过计算出轮式机器人当前速度方向与当前目标位置方向之间的夹角，记为θ，当θ较大时，以最大加速度进行减速，从而削弱当速度方向与目标方向不一致时，转向渐变引发的垂直于路径方向的偏移。

第二阶段直线路径，为了保证移动过程的平稳性，直线路径与第三阶段中的小圆弧路径尽量相切，直线路径的目标点为切点，可以通过一定的几何关系解算得到。另一方面，为了最大程度上优化轮式机器人的功能，在满足最大加速度以及最大速度约束的前提下，先以最大加速度加速到最大速度再以负向的最大加速度减速到目标速度。

第三阶段小圆弧路径，此阶段的目标即为最终目的地，轮式机器人的速度大小保持目标速度大小恒定不变，在满足最大离心加速度的条件下，找到最小半径，尽量减小整个路径占用的空间。

整体上看，轮式机器人的移动路径规划是将整个过程分为了三个阶段，但实际上移动路径规划是在不断修正进行的，每历元根据当前时刻位置，速度，姿态，判断具体执行第几个阶段的命令，直到最终到达目标状态。

因此本发明提出，对移动路径规划的不断修正以及每历元进行阶段判断，实现方式如下：1)当相邻两历元位置变化方向与切点目标位置方向相差较大，且与目的地的距离大于小圆弧段最小半径时，标识第一阶段。

2)当相邻两历元位置变化方向与切点目标位置方向相差较小，且与目的地的距离大于小圆弧段最小半径时，标识第二阶段。

3)切点目标位置位于相邻两历元某中间位置时，标识第三阶段。

如图1所示，实施例定义了此运动模型涉及的两个坐标系：

1)局部坐标系

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

2)全局坐标系

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

同时，定义了轮式机器人的速度和姿态

1)轮式机器人的速度

在局部坐标系下，表示为(v,α)。α∈(-π,π]，为轮式机器人速度矢量与局部坐标系X轴正方向的夹角，一、二象限为正，三、四象限为负，v是速度大小。

2)轮式机器人的姿态

在全局坐标系下，表示为β。β∈(-π,π]，移动主体正方向与全局坐标系X轴正方向的夹角，一、二象限为正，三、四象限为负。

如图2所示，实施例中整体移动路径规划的路径如下：

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

在速度压制部分，通过最大加速度进行减速只能使得垂直方向的偏移削弱，无法消除。

在直线路径过程中，包含着一个加减速过程，加减速阶段的判断通过当前速度，目标速度，当前轮式机器人与目标位置之间的距离，以及最大加速度来进行判断。

对于轮式机器人来说，当接近于切点C时，轮式机器人的姿态、速度大小都已经达到目的地的要求，小圆弧阶段保持固定的姿态、固定的速度大小，只调整速度的方向。

实施例中，移动路径规划的具体步骤如下：

1)输入初始位置、速度、姿态，以及目标位置、速度、姿态

本步骤确定运动主体的起始状态以及目标状态，均包括位置、速度、姿态三类信息。

为更加周全细致的考虑各种情况的适用性，对运动过程进行初步预估，并对特殊情况进行处理。

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

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

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

2)计算小圆弧半径以及对于小圆弧路径圆心的确定，进而唯一确定切点

本步骤根据目标速度以及轮式机器人最大离心加速度的限制得到小圆弧半径的最小值，并根据轮式机器人当前的状态信息在一定的几何关系基础上计算出切点目标信息。

a)小圆弧半径R

V_B是目标速度大小

a_c是轮式机器人最大离心加速度，取决于轮式机器人的物理特性

b)确定小圆弧圆心坐标O:(X_O,Y_O)_Global

如图3中已知目的地B点的状态信息：B:(X_B,Y_B)_Global、(V_B,α_B)_Local、(β_B)_Local。则全局坐标系下B点速度方向α_{B_Glabal}的计算如下：

继而根据圆弧半径以及目标速度方向可以得到如图4中所示两个大小相同且相切的圆。圆心O1、O2的位置信息如下：

R为a)中计算得到的小圆弧段半径

在二者中就近选择一个圆心：

AngleABX＝arctan(Y_A-Y_B,X_A-X_B)

将AngleO1BX归算到(-π,π)：

if AngleO1BX>2π,AngleO1BX＝AngleO1BX-2π；

if AngleO1BX<-2π,AngleO1BX＝AngleO1BX+2π；

AngleABO1＝AngleABX-AngleO1BX

将AngleABO1归算到(-π,π)：

if AngleABO1>2π,AngleABO1＝AngleABO1-2π；

if AngleABO1<-2π,AngleABO1＝AngleABO1+2π；

判断|AngleABO1|与

的关系：

圆心为O1；否则，圆心为O2

其本质上是判断O1、O2中处于起始点和目标点中间的圆心位置，将其记作

O:(X_O,Y_O)_Global

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

如图5中所示切点几何关系示意图

切点C的速度大小V_c为：

V_c＝V_B

切点C的姿态β_c为：

β_c＝β_B

下面求得切点C的位置C:(X_C,Y_C)_Global

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

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

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

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

其中

旋转矩阵

由以下公式可将新坐标系下的坐标转换到全局坐标系中：

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

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

为了使轮式机器人能够以小半圆弧行进，且达到目标要求的速度方向，切点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

3)判断轮式机器人此时所处的阶段并执行相应的指令

若此时上一历元的状态为空，直接进入S3-1，若不为空，则通过相邻两个历元的状态信息对当前时刻轮式机器人所处的阶段进行判断，并做出相应指令。判断条件及执行指令如下：

S3-1当相邻两历元位置变化方向与切点目标位置方向相差较大，且与目的地的距离大与小圆弧段最小半径时，标识第一阶段。

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

S3-2当相邻两历元位置变化方向与切点目标位置方向相差较小，且与目的地的距离大于小圆弧段最小半径时，标识第二阶段。

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

S3-3当目的地之间的距离小于等于小圆弧段最小半径时，标识第三阶段。

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

实施例中，将轮式机器人上一历元位置标记为A1:(X_A1,Y_A1)_Global，当前历元位置标记为A2:(X_A2,Y_A2)_Global，切点位置为C:(XC,Y_C)_Global，目的地为B:(X_B,Y_B)_Global。

全局坐标系下：

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

切点位置方向γ_AC为

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

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

由于轮式机器人的实际运动状态受到各种复杂情况的影响，难免产生微小的偏差，在判断当前速度方向是否与切点目标位置方向相同时，不能够简单将两者完全相等作为判断条件，需要设立一个较小的许可角度偏差阈值，使得两角度偏差在此误差许可范围内时，则视为两角度相等。

本实施例假设δ为许可角度偏差阈值，为一个较小且接近于0的正数，具体实施时，可以预先设置为经验值。基于许可角度偏差阈值判断得到三个阶段实现如下：

a)若

sin|γ_A12-γ_AC|>δ且D_AC>R

则表示轮式机器人当前运动方向与切点目标位置方向不同，且与目的地的距离大于小圆弧段最小半径，标识为第一阶段

b)若

sin|γ_A12-γ_AC|≤δ且D_AC>R

则表示轮式机器人当前运动方向与切点目标位置方向相同，且与目的地的距离大于小圆弧段最小半径，标识第二阶段。

c)若满足

cosγ_{A1_C_A2}<0

则表示A1、A2与切点C形成的张角为钝角，也就是A1、A2位于切点C异侧，此时标识第三阶段。

根据轮式机器人此时所处的阶段，执行相应的任务实现如下：

a)若为第一阶段，则进行速度压制，如图6中所示。

当前时刻轮式机器人的运动速度大小为V_A，轮式机器人目标速度大小为V_B。根据3)中得到的轮式机器人与切点目标之间的距离D_AC。计算AC段的最大速度V_{AC_Max}，其中，a为轮式机器人运动的最大加速度，由轮式机器人物理特性决定。由公式：

得到：

若上一历元情况未知，则通过当前历元轮式机器人的姿态β_A以及速度方向α_A计算轮式机器人当前时刻的运动方向γ_A12：

之前已得到轮式机器人当前时刻的运动方向γ_A12和切点位置方向γ_AC，计算当前时刻轮式机器人运动速度方向与切点目标位置方向的角度差异，得到理想速度的权值K为：

K＝cos|γ_A1-γ_AC|

此时理想速度V_A′为：

V_A′＝V_{AC_Max}×K

对当前速度V_A进行加减速，使其到达理想速度，其中t为相邻两历元时间间隔，a为轮式机器人运动的最大加速度：

ifV_A+at<V_A′，V_A＝V_A+at

ifV_A<V_A′<V_A+at，V_A＝V_A′

ifV_A-at<V_A′<V_A，V_A＝V_A′

ifV_A′<V_A-at，V_A＝V_A-at

b)直线路径下的加减速详细判断过程，其中，a为轮式机器人运动的最大加速度，V_{AC_Max}为a)中计算得到的AC段轮式机器人运动的最大速度。

已知当前轮式机器人的运动速度V_A，以及目标速度V_C的情况下，减速所需要的最小距离S为：

通过判断此时轮式机器人至切点的距离D_AC与S的大小以及V_A与V_{AC_Max}的大小关系，来确定速度大小的变化：

若D_AC<S则以-a减速

若D_AC>S且V_A<V_{AC_Max}，则以a加速

若D_AC>S且V_A≥V_{AC_Max}，则将速度大小设置为V_{AC_Max}

同时，边走直线加减速边调整姿态，将轮式机器人质心速度大小和方向分解到各转向控制部件，保持速度方向不改变的前提下，各转向控制部分在原有速度大小基础上进行微小的改变，来实现姿态的调整。

c)小圆弧路径部分保持速度大小不变，调整速度方向

轮式机器人质心速度、姿态保持不变，通过平台物理结构，将质心速度投射到各个转向控制部分，计算得到能够保持轮式机器人质心沿固定弧线行进的轮子速度以及转向角度数据。优选建议的轮子参数计算步骤如下：

1、计算当前时刻小车质心运动的速度方向。已知起始点A，末位置点B，末速度方向，若末速度方向与AB方向平行，则A点时刻小车的速度方向即为末速度方向；否则计算经过A、B点，且与末速度方向相切的圆形路径，A点时刻小车的速度方向与此圆相切。

2、计算当前时刻小车质心角速度大小。根据当前速度以及距离，近似得到运动到目的地所需要的时间。初、末姿态的角度变化大小除上此时长得到当前时刻小车质心运动角度。

3、此时，将A点时刻小车质心的速度方向，以及质心速度大小和质心角速度大小，通过小车轮子与质心之间的位置关系对车轮侧向受力情况进行约束，使车轮满足侧向不受力的条件，确定各个车轮当前的速度大小和转向角度。

4)阶段任务执行完毕后，判断是否抵达最终目的地

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

相邻两历元轮式机器人到目的地方向的夹角为

若满足：

cosγ_{A1_B_A2}<0

则A1、A2位于目的地B异侧，表明轮式机器人到达了目的地；否则从步骤1开始继续进行，直到最终抵达目的地。

具体实施时，可采用计算机软件实现以上流程的自动运行。

应当理解的是，上述针对较佳实施例的描述较为详细，并不能因此而认为是对本发明专利保护范围的限制，本领域的普通技术人员在本发明的启示下，在不脱离本发明权利要求所保护的情况下，还可以做出替换或变形，均落入本发明的保护范围之内，本发明的请求保护范围应以所附权利要求为准。

Claims (5)

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开始进行下一历元的处理。

2.根据权利要求1所述分阶段的轮式机器人移动路径规划方法，其特征在于：判断阶段的实现如下，

1)当相邻两历元位置变化方向与切点目标位置方向相差大于预设的许可角度偏差阈值，且与目的地的距离大于小圆弧段最小半径时，标识第一阶段，表示速度压制阶段；

2)当相邻两历元位置变化方向与切点目标位置方向相差小于等于预设的许可角度偏差阈值，且与目的地的距离大于小圆弧段最小半径时，标识第二阶段，表示直线段阶段；

3)切点目标位置位于相邻两历元某中间位置时，标识第三阶段，表示小圆弧段阶段。

3.根据权利要求2所述分阶段的轮式机器人移动路径规划方法，其特征在于：设δ为许可角度偏差阈值，为一个较小且接近于0的正数，基于许可角度偏差阈值判断得到三个阶段实现如下：

若

sin|γ_A12-γ_AC|＞δ且D_AC＞R

则表示轮式机器人当前运动方向与切点目标位置方向不同，且与目的地的距离大于小圆弧段最小半径，标识为第一阶段；

若

sin|γ_A12-γ_AC|≤δ且D_AC＞R

则表示轮式机器人当前运动方向与切点目标位置方向相同，且与目的地的距离大于小圆弧段最小半径，标识第二阶段；

若满足

cosγ_{A1_C_A2}＜0

则表示A1、A2与切点C形成的张角为钝角，也就是A1、A2位于切点C异侧，此时标识第三阶段；

根据轮式机器人此时所处的阶段，执行相应的任务实现如下：

a)若为第一阶段，则进行速度压制，当前时刻轮式机器人的运动速度大小为V_A，轮式机器人目标速度大小为V_B，根据轮式机器人与切点目标之间的距离D_AC；计算AC段的最大速度V_{AC_Max}，其中，a为轮式机器人运动的最大加速度，由公式：

得到：

若上一历元情况未知，则通过当前历元轮式机器人的姿态β_A以及速度方向α_A计算轮式机器人当前时刻的运动方向γ_A12：

根据轮式机器人当前时刻的运动方向γ_A12和切点位置方向γ_AC，计算当前时刻轮式机器人运动速度方向与切点目标位置方向的角度差异，得到理想速度的权值K为：

K＝cos|γ_A12-γ_AC|

此时理想速度V′_A为：

V′_A＝V_{AC_Max}×K

对当前速度V_A进行加减速，使其到达理想速度，其中t为相邻两历元时间间隔，a为轮式机器人运动的最大加速度：

if V_A+at＜V′_A，V_A＝V_A+at

if V_A＜V′_A＜V_A+at，V_A＝V′_A

if V_A-at＜V′_A＜V_A，V_A＝V′_A

if V′_A＜V_A-at，V_A＝V_A-at

b)直线路径下的加减速详细判断过程，其中，a为轮式机器人运动的最大加速度，V_{AC_Max}为a)中计算得到的AC段轮式机器人运动的最大速度；

已知当前轮式机器人的运动速度V_A，以及目标速度V_C的情况下，减速所需要的最小距离S为：

通过判断此时轮式机器人至切点的距离D_AC与S的大小以及V_A与V_{AC_Max}的大小关系，来确定速度大小的变化：

若D_AC＜S则以-a减速

若D_AC＞S且V_A＜V_{AC_Max}，则以a加速

若D_AC＞S且V_A≥V_{AC_Max}，则将速度大小设置为V_{AC_Max}

同时，边走直线加减速边调整姿态，将轮式机器人质心速度大小和方向分解到各转向控制部件，保持速度方向不改变的前提下，各转向控制部分在原有速度大小基础上进行微小的改变，来实现姿态的调整；

c)小圆弧路径部分保持速度大小不变，调整速度方向。

4.根据权利要求3所述分阶段的轮式机器人移动路径规划方法，其特征在于：

轮式机器人质心速度、姿态保持不变，通过平台物理结构，将质心速度投射到各个转向控制部分，计算得到能够保持轮式机器人质心沿固定弧线行进的轮子速度以及转向角度数据；轮子参数计算步骤如下：

1)计算当前时刻小车质心运动的速度方向，已知起始点A，末位置点B，末速度方向，若末速度方向与AB方向平行，则A点时刻小车的速度方向即为末速度方向；否则计算经过A、B点，且与末速度方向相切的圆形路径，A点时刻小车的速度方向与此圆相切；

2)计算当前时刻小车质心角速度大小，根据当前速度以及距离，近似得到运动到目的地所需要的时间，初、末姿态的角度变化大小除以时长得到当前时刻小车质心运动角度；

3)此时，将A点时刻小车质心的速度方向，以及质心速度大小和质心角速度大小，通过小车轮子与质心之间的位置关系对车轮侧向受力情况进行约束，使车轮满足侧向不受力的条件，确定各个车轮当前的速度大小和转向角度。

5.根据权利要求3所述分阶段的轮式机器人移动路径规划方法，其特征在于：步骤S4判断到达终点实现方式如下，

相邻两历元轮式机器人到目的地方向的夹角为

若满足：

cosγ_{A1_B_A2}＜0

则A1、A2位于目的地B异侧，表明轮式机器人到达了目的地；否则从步骤S1开始继续进行，直到最终抵达目的地。

Images