[发明专利]一种巡视器航向到位控制轨迹确定方法

说明书

技术领域

本发明涉及一种巡视器航向到位控制轨迹确定方法，属于移动机器人导航控制领域。

背景技术

以指定航向到达某个指定位置，是巡视器导航控制要求之一。在当前位置和航向已知的情况下，需设计合理运动轨迹，使其移动至指定目标位置，且到达该位置时航向角为指定值。控制轨迹应满足：第一，完整运动轨迹由一个或多个弧形轨迹(或直线)组成；第二，弧形轨迹(或直线)数量尽可能少；第三，轨迹运动平滑，避免曲率过大的情况。

已有移动机器人的运动控制方法大多采用点对点运动控制方式，保证点到位精度，没有考虑到位后的航向问题。

发明内容

本发明要解决的技术问题是：为克服现有技术的不足，提出一种巡视器航向到位控制轨迹确定方法，以使得巡视器在满足航向要求的同时，能够以一到两条弧形轨迹或直线平滑运动至目标位置。

本发明的技术解决方案：

一种巡视器航向到位控制轨迹确定方法，具体步骤如下：

(1)航向到位初始轨迹判断

记巡视器当前点为A点，目标点为B点；

过A、B两点做弧，使其与A点偏航方向相切，记为弧1，对应曲率为C_A；过A、B两点做弧，使其与B点偏航方向相切，记为弧2，对应曲率为C_B；

若C_A＝C_B，弧1与弧2重合，则A、B两点之间的运动轨迹为一条弧形或直线，该轨迹的期望曲率为C＝C_A＝C_B，期望弧长即为弧1或弧2的弧长，轨迹确定完成；

若C_A≠C_B，则转入步骤(2)；

(2)选取平滑轨迹中途点

过A、B两点做弧，使其曲率为C_M＝(C_A+C_B)/2，记为弧3，取弧3的中点为轨迹中途点，记为点M；

(3)确定最终轨迹

比较|C_A|和|C_B|，

若|C_A|≤|C_B|，则过B、M两点做圆，使其与B点偏航方向相切，记其圆点为O_B，过A点做圆，使其与圆O_B-B及A点偏航方向相切，记其圆点为O_A，圆O_A-A与圆O_B-B的切点记为P，O_A、P、O_B共线；

若|C_A|>|C_B|，则过A、M两点做圆，使其与A点偏航方向相切，记其圆点为O_A，过B点做圆，使其与圆O_A-A及B点偏航方向相切，记其圆点为O_B，圆O_A-A与圆O_B-B的切点记为P，O_A、P、O_B共线；

此时A、B两点之间的运动轨迹由两条弧形或直线组成，分别为AP和BP，轨迹确定完成。

曲率的正负符号定义为：若弧形轨迹圆点位于当前点右侧，此时弧形对应巡视器右转，曲率为正；若弧形轨迹圆点位于当前点左侧，此时弧形对应巡视器左转，曲率为负；若弧形轨迹圆点为无限远点，轨迹变为直线，对应巡视器直行，曲率为零。

本发明与现有技术相比的优点在于：

(1)本发明将点到位和航向控制分开，点到位后再通过原地转向方式调整航向，降低了探测效率，增加了控制复杂性；

(2)本发明的完整运动轨迹由一个或多个弧形轨迹或直线组成、轨迹运动平滑，不会出现曲率过大导致控制偏差增大的情况。

附图说明

图1为巡视器当前位置航向与目标位置航向关系示意图；

图2为确定轨迹中途点M示意图；

图3为确定移动轨迹示意图；

图4为确定运动轨迹示意图。

具体实施方式

下面结合附图及具体实施例对本发明进行详细描述。

已知巡视器起始点位置及航向为(x₁,y₁,ψ₁)＝(0,0,0°)，目标点位置及航向为(x₂,y₂,ψ₂)＝(-1,5,30°)。确定巡视器的期望运动轨迹，步骤如下：