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

权利要求书

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_By_A-x_Ay_B＝0

S23：令a＝y_B-y_A,b＝x_A-x_B,c＝x_By_A-x_Ay_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_By_A-x_Ay_B；

S42：直线AB的垂线方程为：

bx-ay+m＝0；

其中m为参变量，将D点坐标带入方程可得：

m＝ay_D-bx_D；

通过方程组：

得出交点E的坐标为

6.根据权利要求5所述的一种新的机器人航向角偏置的自动迭代修正方法，其特征在于：所述步骤S6中，根据θ的值更新补偿后机器人的航向角即：