一种巡视器航向到位控制轨迹确定方法
【技术领域】
[0001]本发明涉及一种巡视器航向到位控制轨迹确定方法，属于移动机器人导航控制领域。
【背景技术】
[0002]以指定航向到达某个指定位置，是巡视器导航控制要求之一。在当前位置和航向已知的情况下，需设计合理运动轨迹，使其移动至指定目标位置，且到达该位置时航向角为指定值。控制轨迹应满足:第一，完整运动轨迹由一个或多个弧形轨迹(或直线)组成；第二，弧形轨迹(或直线)数量尽可能少；第三，轨迹运动平滑，避免曲率过大的情况。
[0003]已有移动机器人的运动控制方法大多采用点对点运动控制方式，保证点到位精度，没有考虑到位后的航向问题。
【发明内容】
[0004]本发明要解决的技术问题是:为克服现有技术的不足，提出一种巡视器航向到位控制轨迹确定方法，以使得巡视器在满足航向要求的同时，能够以一到两条弧形轨迹或直线平滑运动至目标位置。
[0005]本发明的技术解决方案:
[0006]一种巡视器航向到位控制轨迹确定方法，具体步骤如下:
[0007](I)航向到位初始轨迹判断
[0008]记巡视器当前点为A点，目标点为B点；
[0009]过A、B两点做弧，使其与A点偏航方向相切，记为弧1，对应曲率为Ca;过A、B两点做弧，使其与B点偏航方向相切，记为弧2，对应曲率为Cb;
[0010]若Ca= C B，弧I与弧2重合，则A、B两点之间的运动轨迹为一条弧形或直线，该轨迹的期望曲率为C = Ca= Cb，期望弧长即为弧I或弧2的弧长，轨迹确定完成；
[0011 ] 若C C B，则转入步骤(2)；
[0012](2)选取平滑轨迹中途点
[0013]过A、B两点做弧，使其曲率为Cm= (CA+CB)/2，记为弧3，取弧3的中点为轨迹中途点，记为点M ；
[0014](3)确定最终轨迹
[0015]比较IcaI 和 IcB|，
[0016]若IcaI ( I Cb I，则过B、M两点做圆，使其与B点偏航方向相切，记其圆点为Ob，过A点做圆，使其与圆Ob-B及A点偏航方向相切，记其圆点为0A，圆Oa-A与圆Ob-B的切点记为P，0A、P、0B共线；
[0017]若I CA| > I Cb I，则过A、M两点做圆，使其与A点偏航方向相切，记其圆点为0A，过B点做圆，使其与圆Oa-A及B点偏航方向相切，记其圆点为0B，圆Oa-A与圆Ob-B的切点记为P，0A、P、0B共线；
[0018]此时A、B两点之间的运动轨迹由两条弧形或直线组成，分别为AP和BP，轨迹确定完成。
[0019]曲率的正负符号定义为:若弧形轨迹圆点位于当前点右侧，此时弧形对应巡视器右转，曲率为正；若弧形轨迹圆点位于当前点左侧，此时弧形对应巡视器左转，曲率为负；若弧形轨迹圆点为无限远点，轨迹变为直线，对应巡视器直行，曲率为零。
[0020]本发明与现有技术相比的优点在于:
[0021](I)本发明将点到位和航向控制分开，点到位后再通过原地转向方式调整航向，降低了探测效率，增加了控制复杂性；
[0022](2)本发明的完整运动轨迹由一个或多个弧形轨迹或直线组成、轨迹运动平滑，不会出现曲率过大导致控制偏差增大的情况。
【附图说明】
[0023]图1为巡视器当前位置航向与目标位置航向关系示意图；
[0024]图2为确定轨迹中途点M示意图；
[0025]图3为确定移动轨迹示意图；
[0026]图4为确定运动轨迹示意图。
【具体实施方式】
[0027]下面结合附图及具体实施例对本发明进行详细描述。
[0028]已知巡视器起始点位置及航向为(Xl，Y1, ^1) = (O, 0,0° )，目标点位置及航向为(x2, y2) ￠2) = (-1,5,30。) O确定巡视器的期望运动轨迹，步骤如下:
[0029](I)计算边界曲率
[0030]记巡视器当前点为A点，目标点为B点。
[0031]过A、B两点做弧，使其与A点偏航方向相切，记为弧1，对应曲率(；为-0.0769 ;过A、B两点做弧，使其与B点偏航方向相切，记为弧2，对应曲率(；为0.2589。
[0032](2)确定轨迹中途点M
[0033]过A、B两点做弧，曲率Cm为0.0910，记为弧3，取弧3的中点为轨迹中途点，记为点 M，坐标为(2.4412，-0.7940)。
[0034](3)确定移动轨迹
[0035]比较I Ca I和I Cb I，有I Ca I ( I Cb |，则过B、M两点做圆，使其与B点偏航方向相切，圆点ObS (3.8699，0.9574)，过A点做圆，使其与圆O b_B及A点偏航方向相切，圆点04为(O, -4.1387)，圆 Oa-A 与圆 Ob-B 的切点 P 为(2.5030，-0.8426)，Oa、P、Ob共线。
[0036]此时A、B两点之间的运动轨迹由两条弧形组成，分别为弧AP和弧BP，AP的曲率为-0.2416，弧长为2.6880，BP的曲率为0.4424，弧长为2.6514。轨迹确定完成。运动轨迹如图4所示。
[0037]具体要求可明确为:已知巡视器当前点的位置坐标及偏航方向(Xl，yi，及目标点的位置坐标及偏航方向(χ2，y2, Φ O，如图1所示，计算连接当前点和目标点的一个或多个弧形轨迹(或直线)的曲率及弧长。
[0038]其中曲率的正负符号定义为:若弧形轨迹圆点位于当前点右侧，此时弧形对应巡视器右转，曲率为正；若弧形轨迹圆点位于当前点左侧，此时弧形对应巡视器左转，曲率为负；若弧形轨迹圆点为无限远点，轨迹变为直线，对应巡视器直行，曲率为零。
【主权项】
1.一种巡视器航向到位控制轨迹确定方法，其特征在于，具体步骤如下: (1)航向到位初始轨迹判断 记巡视器当前点为A点，目标点为B点； 过A、B两点做弧，使其与A点偏航方向相切，记为弧1，对应曲率为Ca;过A、B两点做弧，使其与B点偏航方向相切，记为弧2，对应曲率为Cb; 若Ca= C B，弧I与弧2重合，则A、B两点之间的运动轨迹为一条弧形或直线，该轨迹的期望曲率为C = Ca= Cb，期望弧长即为弧I或弧2的弧长，轨迹确定完成； 若CA# C B，则转入步骤⑵； (2)选取平滑轨迹中途点 过A、B两点做弧，使其曲率为Cm= (Ca+Cb)/2，记为弧3，取弧3的中点为轨迹中途点，记为点M ； (3)确定最终轨迹 比较|cA|和IcB|， 若|cA| ( |cB|，则过B、μ两点做圆，使其与B点偏航方向相切，记其圆点为oB，过A点做圆，使其与圆Ob-B及A点偏航方向相切，记其圆点为0A，圆Oa-A与圆Ob-B的切点记为P，0A、P、0B共线； 若I Ca I > I Cb I，则过A、M两点做圆，使其与A点偏航方向相切，记其圆点为0A，过B点做圆，使其与圆Oa-A及B点偏航方向相切，记其圆点为0B，圆Oa-A与圆Ob-B的切点记为P，0A、P、Ob共线； 此时A、B两点之间的运动轨迹由两条弧形或直线组成，分别为AP和BP，轨迹确定完成。
2.如权利要求1所述的一种巡视器航向到位控制轨迹确定方法，其特征在于，曲率的正负符号定义为:若弧形轨迹圆点位于当前点右侧，此时弧形对应巡视器右转，曲率为正；若弧形轨迹圆点位于当前点左侧，此时弧形对应巡视器左转，曲率为负；若弧形轨迹圆点为无限远点，轨迹变为直线，对应巡视器直行，曲率为零。
【专利摘要】本发明公开了一种巡视器航向到位控制轨迹确定方法，实现步骤包括：航向到位初始轨迹判断，选取平滑轨迹中途点，确定最终轨迹。本发明将点到位和航向控制分开，点到位后再通过原地转向方式调整航向，降低了探测效率，增加了控制复杂性。
【IPC分类】G05D1-10
【公开号】CN104571129
【申请号】CN201410827873
【发明人】邢琰, 滕宝毅, 刘祥, 毛晓艳, 贾永
【申请人】北京控制工程研究所
【公开日】2015年4月29日
【申请日】2014年12月26日