一种新的机器人航向角偏置的自动迭代修正方法与流程

阅读：评论：0

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

背景技术：

2.机器人在平面中做定位导航控制，首先要解决的是沿两点直线行走，对于不能横向平移的机器人平台，如两轮差动和阿克曼结构的机器人，最基本的两点直线闭环控制算法如下：
3.如图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点，不过控制上可以设定一个阈值，当机器人与目标点的位置差小于这个阈值，就认为机器人完成了轨迹。
4.为了让机器人尽量走直线，就需要想办法测出这个θ的值，然后对机器人航向角二次补偿，使得和尽量一致，现有的方式是对机器人当前位置点到直线段ab的距离偏移量dh进行pid调节，以pid的输出作为θ对机器人航向角进行二次补偿，但是此方法也有弊端，由于横向偏移量dh的变化明显滞后于航向角变化，这个pid控制系统很容易震荡，导致
机器人沿直线ab左右摇摆，形成s型轨迹。经过发明人长期研究，发明了一种新的机器人航向角偏置的自动迭代修正方法。

技术实现要素：

5.本发明的目的在于克服现有技术的缺点，提供一种新的机器人航向角偏置的自动迭代修正方法。
6.本发明的目的通过以下技术方案来实现：一种新的机器人航向角偏置的自动迭代修正方法，包括以下步骤：
7.s1：记录机器人从a点到b点的运动轨迹，得到一组离散的按时间分布的坐标点，这组坐标点组成点集p；
8.s2：对点集p中的每个点，计算到线段ab的距离，得到距离的集合l；
9.s3：对集合l进行排序，选出离线段ab最远的点，该点设为d，做经过点d的轨迹曲线的切线df，则df与线段ab平行，且df与db的夹角等于θ；
10.s4：经过点d作线段ab的垂线并交ab于点e，计算点e的坐标和直角三角形bde的三边长度；
11.s5：计算∠dbe，
[0012][0013]
则θ＝∠dbe；
[0014]
s6：根据θ的值更新补偿后机器人的航向角
[0015]
s7：在下一段两点直线段中应用新的机器人航向角进行控制，并重复步骤s1～步骤s6，如此迭代进行自动修正。
[0016]
优选的，步骤s1中，密集记录机器人从a点到b点的运动轨迹。
[0017]
优选的，步骤s2中，点集p中的每个点，计算到线段ab的距离的步骤如下：
[0018]
s21：设集合p中第i个点pi的坐标为(xi，yi)，点a和点b的坐标分别为(xa，ya)和(xb，yb)；
[0019]
s22：经过点ab的两点式直线方程为转化为一般式为：
[0020]
(y
b-ya)x+(x
a-xb)y+xby
a-xayb＝0
[0021]
s23：令a＝y
b-ya，b＝x
a-xb，c＝xby
a-xayb，则点pi到线段ab的距离li为：
[0022][0023]
优选的，步骤s3中，以a点为原点，ab所在直线为x轴建立一个新坐标系，对曲线adb沿x轴方向求导，从a点到d点，导数大于零并逐渐减小，从d点到b点，导数小于零，由于曲线的导数是连续的，即d点的导数为0，得出切线df的斜率为0，与线段ab平行。
[0024]
优选的，步骤s4中，计算点e的坐标步骤如下：
[0025]
s41：设点a的坐标为(xa，ya)，点b的坐标为(xb，yb)，点d的坐标为(xd，yd)，根据所述步骤s22和所述步骤s23可知，ab的直线方程为
[0026]
ax+by+c＝0；
[0027]
其中a＝y
b-ya，b＝x
a-xb，c＝xby
a-xayb；
[0028]
s42：直线ab的垂线方程为：
[0029]
bx-ay+m＝0；
[0030]
其中m为参变量，将d点坐标带入方程可得：
[0031]
m＝ay
d-bxd；
[0032]
通过方程组：
[0033][0034]
得出交点e的坐标为
[0035]
优选的，步骤s6中，根据θ的值更新补偿后机器人的航向角即：
[0036][0037]
本发明具有以下优点：本发明根据机器人每段行走轨迹，计算出补偿后机器人航向与工作坐标系航向的偏差θ，并在下一段轨迹开始前更新从而使每走一段均更新一次补偿值，如此循环迭代，就可以让机器人不断自动修正航向角，使轨迹越来越接近于直线。
附图说明
[0038]
图1为自动迭代修正方法数学平面的结构示意图；
[0039]
图2为df与ab位置关系的结构示意图；
[0040]
图3为基本两点直线闭环控制算法的结构示意图；
[0041]
图4为机器人航向角与坐标系航向角位置关系的结构示意图；
[0042]
图5为控制算法下实际机器人运动轨迹的结构示意图；
具体实施方式
[0043]
为使本发明实施方式的目的、技术方案和优点更加清楚，下面将结合本发明实施方式中的附图，对本发明实施方式中的技术方案进行清楚、完整地描述，显然，所描述的实施方式是本发明一部分实施方式，而不是全部的实施方式。通常在此处附图中描述和示出的本发明实施方式的组件可以以各种不同的配置来布置和设计。
[0044]
因此，以下对在附图中提供的本发明的实施方式的详细描述并非旨在限制要求保护的本发明的范围，而是仅仅表示本发明的选定实施方式。基于本发明中的实施方式，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施方式，都属于本发明保护的范围。
[0045]
需要说明的是，在不冲突的情况下，本发明中的实施方式及实施方式中的特征可以相互组合。
[0046]
应注意到：相似的标号和字母在下面的附图中表示类似项，因此，一旦某一项在一个附图中被定义，则在随后的附图中不需要对其进行进一步定义和解释。
[0047]
在本发明的描述中，需要说明的是，术语“中心”、“上”、“下”、“左”、“右”、“竖直”、“水平”、“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系，或者是该发明产品使用时惯常摆放的方位或位置关系，或者是本领域技术人员惯常理解的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。此外，术语“第一”、“第二”等仅用于区分描述，而不能理解为指示或暗示相对重要性。
[0048]
在本发明的描述中，还需要说明的是，除非另有明确的规定和限定，术语“设置”、“安装”、“相连”、“连接”应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或一体地连接；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通。对于本领域的普通技术人员而言，可以具体情况理解上述术语在本发明中的具体含义。
[0049]
在本实施例中，如图1所示，一种新的机器人航向角偏置的自动迭代修正方法，其特征在于：包括以下步骤：
[0050]
s1：记录机器人从a点到b点的运动轨迹，得到一组离散的按时间分布的坐标点，这组坐标点组成点集p；优选的，步骤s1中，密集记录机器人从a点到b点的运动轨迹。
[0051]
s2：对点集p中的每个点，计算到线段ab的距离，得到距离的集合l；
[0052]
s3：对集合l进行排序，选出离线段ab最远的点，该点设为d，做经过点d的轨迹曲线的切线df，则df与线段ab平行，且df与db的夹角等于θ；
[0053]
s4：经过点d作线段ab的垂线并交ab于点e，计算点e的坐标和直角三角形bde的三边长度；具体地说，由于de垂直于ab，而df与ab平行，所以de也垂直于df。
[0054]
s5：计算∠dbe，
[0055][0056]
则θ＝∠dbe；
[0057]
s6：根据θ的值更新补偿后机器人的航向角
[0058]
s7：在下一段两点直线段中应用新的机器人航向角进行控制，并重复步骤s1～步骤s6，如此迭代进行自动修正。根据机器人每段行走轨迹，计算出补偿后机器人航向与工作坐标系航向的偏差θ，并在下一段轨迹开始前更新从而使每走一段均更新一次补偿值，如此循环迭代，就可以让机器人不断自动修正航向角，使轨迹越来越接近于直线。
[0059]
进一步的，步骤s2中，点集p中的每个点，计算到线段ab的距离的步骤如下：
[0060]
s21：设集合p中第i个点pi的坐标为(xi，yi)，点a和点b的坐标分别为(xa,ya)和(xb,yb)；
[0061]
s22：经过点ab的两点式直线方程为转化为一般式为：
[0062]
(y
b-ya)x+(x
a-xb)y+xby
a-xayb＝0
[0063]
s23：令a＝y
b-ya，b＝x
a-xb，c＝xby
a-xayb，则点pi到线段ab的距离li为：
[0064][0065]
再进一步的，如图2所示，步骤s3中，以a点为原点，ab所在直线为x轴建立一个新坐
标系，对曲线adb沿x轴方向求导，从a点到d点，导数大于零并逐渐减小，从d点到b点，导数小于零，由于曲线的导数是连续的，即d点的导数为0，得出切线df的斜率为0，与线段ab平行。具体地说，根据导数的几何意义，切点处导数等于切线斜率，由于d点的导数为0，因而切线df的斜率为0，所以df平行与x轴，即df平行于ab。
[0066]
在本实施例中，步骤s4中，计算点e的坐标步骤如下：
[0067]
s41：设点a的坐标为(xa，ya)，点b的坐标为(xb，yb)，点d的坐标为(xd，yd)，根据所述步骤s22和所述步骤s23可知，ab的直线方程为
[0068]
ax+by+c＝0；
[0069]
其中a＝y
b-ya，b＝x
a-xb，c＝xby
a-xayb；
[0070]
s42：直线ab的垂线方程为：
[0071]
bx-ay+m＝0；
[0072]
其中m为参变量，将d点坐标带入方程可得：
[0073]
m＝ay
d-bxd；
[0074]
通过方程组：
[0075][0076]
得出交点e的坐标为
[0077]
进一步的，步骤s6中，根据θ的值更新补偿后机器人的航向角即：
[0078][0079]
具体地说，在下一段两点直线段中应用新的机器人航向角进行控制，并重复步骤s1～步骤s6，根据下一段的轨迹继续计算θ，然后继续更新如此迭代，即可让机器人根据经验轨迹持续自动修正航向角偏置，使其行走轨迹越来越接近于直线。
[0080]
尽管参照前述实施例对本发明进行了详细的说明，对于本领域的技术人员来说，其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

技术特征：

1.一种新的机器人航向角偏置的自动迭代修正方法，其特征在于：包括以下步骤：s1：记录机器人从a点到b点的运动轨迹，得到一组离散的按时间分布的坐标点，这组坐标点组成点集p；s2：对点集p中的每个点，计算到线段ab的距离，得到距离的集合l；s3：对集合l进行排序，选出离线段ab最远的点，该点设为d，做经过点d的轨迹曲线的切线df，则df与线段ab平行，且df与db的夹角等于θ；s4：经过点d作线段ab的垂线并交ab于点e，计算点e的坐标和直角三角形bde的三边长度；s5：计算∠dbe，则θ＝∠dbe；s6：根据θ的值更新补偿后机器人的航向角s7：在下一段两点直线段中应用新的机器人航向角进行控制，并重复步骤s1～步骤s6，如此迭代进行自动修正。2.根据权利要求1所述的一种新的机器人航向角偏置的自动迭代修正方法，其特征在于：所述步骤s1中，密集记录机器人从a点到b点的运动轨迹。3.根据权利要求2所述的一种新的机器人航向角偏置的自动迭代修正方法，其特征在于：所述步骤s2中，点集p中的每个点，计算到线段ab的距离的步骤如下：s21：设集合p中第i个点p
i
的坐标为(x
i
，y
i
)，点a和点b的坐标分别为(x
a
,y
a
)和(x
b
,y
b
)；s22：经过点ab的两点式直线方程为转化为一般式为：(y
b-y
a
)x+(x
a-x
b
)y+x
b
y
a-x
a
y
b
＝0s23：令a＝y
b-y
a
,b＝x
a-x
b
,c＝x
b
y
a-x
a
y
b
，则点p
i
到线段ab的距离l
i
为：4.根据权利要求3所述的一种新的机器人航向角偏置的自动迭代修正方法，其特征在于：所述步骤s3中，以a点为原点，ab所在直线为x轴建立一个新坐标系，对曲线adb沿x轴方向求导，从a点到d点，导数大于零并逐渐减小，从d点到b点，导数小于零，由于曲线的导数是连续的，即d点的导数为0，得出切线df的斜率为0，与线段ab平行。5.根据权利要求4所述的一种新的机器人航向角偏置的自动迭代修正方法，其特征在于：所述步骤s4中，计算点e的坐标步骤如下：s41：设点a的坐标为(x
a
，y
a
)，点b的坐标为(x
b
，y
b
)，点d的坐标为(x
d
，y
d
)，根据所述步骤s22和所述步骤s23可知，ab的直线方程为ax+by+c＝0；其中a＝y
b-y
a
，b＝x
a-x
b
，c＝x
b
y
a-x
a
y
b
；s42：直线ab的垂线方程为：bx-ay+m＝0；其中m为参变量，将d点坐标带入方程可得：
m＝ay
d-bx
d
；通过方程组：得出交点e的坐标为6.根据权利要求5所述的一种新的机器人航向角偏置的自动迭代修正方法，其特征在于：所述步骤s6中，根据θ的值更新补偿后机器人的航向角即：

技术总结

本发明公开了一种新的机器人航向角偏置的自动迭代修正方法，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，如此迭代自动修正。机器人每段行走轨迹，计算出θ，并在下一段轨迹开始前更新每走一段均更新一次补偿值，如此循环迭代，让机器人不断自动修正航向角，使轨迹接近直线。使轨迹接近直线。使轨迹接近直线。