[发明专利]一种新的机器人航向角偏置的自动迭代修正方法

说明书

本发明公开了一种新的机器人航向角偏置的自动迭代修正方法，S1：记录机器人从A点到B点的运动轨迹，得到一组坐标点，组成点集P；S2：对P中的每个点，计算到线段AB的距离，得到集合L；S3：对L进行排序，选出离线段AB最远的点，为D，经过点D做轨迹曲线的切线DF，与AB平行，DF与DB的夹角等于θ；S4：经过点D作线段AB垂线并交于点E，计算点E坐标；S5：计算∠DBE，θ＝∠DBE；S6：θ值更新补偿后机器人的航向角S7：下一段两点直线段中应用新的进行控制，重复S1～S6，如此迭代自动修正。机器人每段行走轨迹，计算出θ，并在下一段轨迹开始前更新每走一段均更新一次补偿值，如此循环迭代，让机器人不断自动修正航向角，使轨迹接近直线。

技术领域

本发明涉及机器人航向角修正技术领域，特别是一种新的机器人航向角偏置的自动迭代修正方法。

背景技术

机器人在平面中做定位导航控制，首先要解决的是沿两点直线行走，对于不能横向平移的机器人平台，如两轮差动和阿克曼结构的机器人，最基本的两点直线闭环控制算法如下：

如图3所示，假设机器人从A点直线走到B点，将机器人当前位置P与目标点B的距离L作为位置偏差输入PID调节器，经过PID调节器输出得到机器人的线速度；将机器人当前位置P与目标点B连线的方向作为目标航向，目标航向与机器人当前航向的角度偏差d作为另一个PID调节器的输入，经过该PID调节器输出得到机器人的角速度；理想情况下，在行进过程中控制角度偏差d等于0，持续减小位置偏差L，直到L也等于0，机器人就会沿着直线段AB从A走到B。但这种控制方法，有一个前提条件，就是机器人本体的航向与工作坐标系航向必须匹配一致，一般地，工作坐标系中，定义y轴正方向航向为0度，逆时针航向增加，顺时针航向减小，x轴正方向航向为-90度，我们定义为工作坐标系中的航向角。机器人本体也会输出一个机器人航向角，设为一般与工作坐标系航向不一致，所以需要事先标定，计算出两个航向之间的偏置角度并在机器人航向角上加上这个偏置角度进行补偿，设补偿后的机器人航向角为则理想情况下，我们希望补偿后的机器人航向角等于工作坐标系航向角，即但是，受标定精度影响，与还是会存在一个偏差θ，如图4所示，假设指向右上方的箭头是机器人认为的0°航向，而指向正上方的箭头是工作坐标系中对应的0°航向，2者夹角为偏差θ，那么在工作坐标系中，机器人的航向角实际上为-θ，因此，机器人航向角还需要减去一个θ，才能使得两个航向完全重合，即此时和就完全一致了。在偏差θ存在的情况下，采用前面描述的两点直线控制算法，机器人会走歪，轨迹如图5粗线所示，这是一条曲线，图中，A点为起始点，B点为目标点，需要控制机器人从A点走到B点，AB细直线是机器人的理想运动轨迹，粗曲线是机器人实际运动轨迹，在这段曲线上的任意一点P均代表机器人某时刻经过的位置，经过P点的曲线切线方向代表机器人该时刻的实际航向，P与B点的连线方向是机器人期望航向，这两个方向的夹角就是机器人的航向角偏差θ，在偏差θ存在的情况下，机器人的运动轨迹是一个螺旋曲线，其数学描述也比较复杂。理论上机器人最终只能无限接近B点，而到不了B点，不过控制上可以设定一个阈值，当机器人与目标点的位置差小于这个阈值，就认为机器人完成了轨迹。

为了让机器人尽量走直线，就需要想办法测出这个θ的值，然后对机器人航向角二次补偿，使得和尽量一致，现有的方式是对机器人当前位置点到直线段AB的距离偏移量d_h进行PID调节，以PID的输出作为θ对机器人航向角进行二次补偿，但是此方法也有弊端，由于横向偏移量d_h的变化明显滞后于航向角变化，这个PID控制系统很容易震荡，导致机器人沿直线AB左右摇摆，形成S型轨迹。经过发明人长期研究，发明了一种新的机器人航向角偏置的自动迭代修正方法。

发明内容

本发明的目的在于克服现有技术的缺点，提供一种新的机器人航向角偏置的自动迭代修正方法。

本发明的目的通过以下技术方案来实现：一种新的机器人航向角偏置的自动迭代修正方法，包括以下步骤：

S1：记录机器人从A点到B点的运动轨迹，得到一组离散的按时间分布的坐标点，这组坐标点组成点集P；