[发明专利]一种复杂环境下拖挂式移动机器人的运动规划与控制方法

权利要求书

1.一种复杂环境下拖挂式移动机器人的运动规划与控制方法，拖挂式移动机器人包括牵引车和拖车，牵引车与拖车之间通过链接点相连接，链接点处设置有用于测量拖车的车体纵向线相对于牵引车的车体纵向线转动角度的角度传感器；牵引车由后轮驱动、前轮转向，牵引车前转向轮的转向角速度为                                                ，牵引车的纵向速度为，且车轮与地面无滑动；牵引车驱动轮与转向轮之间的距离为，拖车的轮轴中点到链接点的距离为，连接轴的长度为；牵引车的左、右驱动轮可分别进行驱动，并通过左、右驱动轮的不同转速实现转向；牵引车上设置有声呐传感器，可实时检测0～360°空间内的障碍物信息，设声呐传感器的有效检测距离为;

定义为坐标系中牵引车和拖车的位形，=1时表示牵引车的位形，=2时表示拖车的位形；为牵引车后轮轴中心点的坐标，为拖车轮轴中线点的坐标；为方位角，表示车体纵向线相对于坐标系轴正向所旋转的角度；为牵引车的转向角，表示牵引车转向轮与车体纵向线之间的夹角；牵引车可获取自身车体纵向线相对于坐标系轴正向所旋转的角度；、为牵引车左、右驱动轮的速度；

其特征在于，所述拖挂式移动机器人的运动规划与控制方法通过以下步骤来实现：

a).定义初始和终点位形，设初始位形为：=0，=0，==π，记为点；目标位形为=，=，==π，记为点，即要实现拖挂式机器人从点到点运动路径的规划与控制；

b).检测障碍物，在初始位形状态下，牵引车通过声呐传感器检测周围的障碍物，其中表示第个障碍物的位置坐标，为障碍物的置信度；并通过如下公式求出障碍物的置信度：

        （1）

其中，为障碍物距离声呐传感器的距离，为置信度估计参数；

c).障碍物的舍去，对于检测到的第个障碍物，判断步骤b)中所求取的障碍物的置信度，如果＜，则将该置信度对应的障碍物舍去；

d).定义障碍物空间，定义障碍物空间=，其中表示障碍物空间的第个区域，当≠时：=，令=；

e).对于牵引车检测到的障碍物，首先判断=是否成立，如果为空集，则生成区域=，如果不为空集，则执行步骤f)；

f).通过公式（2）判断障碍物是否属于空间中的区域，；

≤      （2）

如果与区域内任一障碍物的距离满足不等式（2），则认为属于区域，令=；否则，则认为不属于区域，生成区域；执行步骤g)；

g).判断障碍物空间是否建立完毕，判断牵引车在当前位形状态下能检测到的所有障碍物的空间是否均已建立完毕，如果均已建立完毕，则执行步骤h)；如果没有建立完毕，则跳转至步骤e)；

h).建立双向搜索随机树，建立快速搜索随机树和，将初始位形、目标位形分别作为、的根节点；

i).随机生成位形，采用快速搜索随机树算法在树和中生成新位形、，执行步骤j)；

j).判断获取的新位形、与障碍物是否冲突，如果冲突，则执行步骤i)；如果不冲突，则新位形、分别保存为树和的叶子节点，执行步骤k)；

k).判断树和是否已连接，判断∈或者∈是否成立，如果成立，则表明树与已经连接，拖挂式移动机器人由初始位形至目标位形的路径规划结束，执行步骤l)；如果不成立，则执行步骤i)；

l).直线路径的拟合，将步骤h)至k)所求出由若干离散的位形点组成的路径，拟合为由多段直线组成的便于拖挂式机器人行走的路径；

m).运动控制，根据步骤l)中确定的多段直线路径，求出相应直线路径下牵引车和拖车的转向角，即可控制拖挂式机器人由初始位形向目标位形运动；

n).判断是否有新的障碍物存在，在拖挂式机器人由初始位形向目标位形运动的过程中，牵引车以一定的周期判断周围是否有新的障碍物出现，如果没有新的障碍物出现，则控制拖挂式机器人按照当前路径运行；如果有新的障碍物出现，则将牵引车当前的位形作为初始位形，执行步骤b)，以对当前路径从新进行规划。

2.根据权利要求1所述的复杂环境下拖挂式移动机器人的运动规划与控制方法，其特征在于，设系统的函数为，步骤i)中新位形和按照如下方法求取：

i-1).首先分别在树和中求解与相距最近的节点和；

i-2).选择控制输入，按照如下计算公式：

     （3）

     （4）

计算出新位形和。