CN108582065B

CN108582065B - 基于fpga的六轴机械臂运动方程逆解求解的方法

Info

Publication number: CN108582065B
Application number: CN201810178011.7A
Authority: CN
Inventors: 黄继业; 谢尚港; 陆燕怡; 高明煜; 陈德平; 杨宇翔; 何志伟
Original assignee: Hangzhou Dianzi University
Current assignee: Hangzhou Dianzi University
Priority date: 2018-03-05
Filing date: 2018-03-05
Publication date: 2021-05-04
Anticipated expiration: 2038-03-05
Also published as: CN108582065A

Abstract

本发明公开了一种基于FPGA的六轴机械臂运动方程逆解快速求解的方法，该方法通过FPGA直接实现逆解计算，根据重新整合后的计算过程，得到相应关节角。首先通过传感器获取机械臂末端目标的运动坐标，输入FPGA中，利用流水线设计与模块时间复用相结合，仅使用2个浮点乘法器、1个浮点除法器、1个浮点开方模块、1个Cordic模块、1个反正切模块，2个浮点加法器，全并行工作，有效降低运动学方程逆解求解所需算子的时钟延时，快速获得逆解(N个角度值)进行输出。从而大大提高了求解运动学方程逆解的速度，并且降低了系统的成本。

Description

基于FPGA的六轴机械臂运动方程逆解求解的方法

技术领域

本发明涉及工业机器人运动控制领域，尤其涉及一种基于FPGA的六轴机械臂运动方程逆解快速求解的方法。

背景技术

在工业机器人运动控制领域，对定位出满足工程要求的机器轴方位的方面有着较为广泛的应用；而实现对机械臂的定位这种功能的关键在于设计并实现一套高效的运动学方程的逆解的快速求解，尤其是当机械臂处于高速运动的状态；在传统的技术中，通常将求解六轴机械臂的运动学方程的逆解放在DSP或者上位机上实现，不仅成本高昂而且速度较慢。

发明内容

本发明针对现有技术的不足，提供了一种基于FPGA的六轴机械臂运动方程逆解快速求解的方法，可以加快求解的速度且降低成本。

为了解决上述技术问题，本发明提供的一种基于FPGA的六轴机械臂运动方程逆解快速求解的方法，包括：

通过机械臂末端目标的位姿矩阵，根据逆解求解要求目标的位置信息以及计算过程，对所有计算过程进行拆解后进行去相关，然后重新整合；

通过FPGA直接实现逆解计算，根据重新整合后的计算过程，得到相应关节角。

该系统包括两个浮点乘法器、一个浮点除法器、一个浮点开方模块、一个 Cordic模块、一个反正切模块和两个浮点加法器；其全并行工作，有效降低运动学方程逆解求解所需算子的计算时间，快速获得逆解(N个角度值)进行输出。

各模块间的数据传输由主时序模块控制，即控制各模块间数据输入从而实现相应计算，计算完成获得的数据作为另一个模块的输入进行新的计算。

上述位置信息为六轴机器臂的位姿信息，同样的可扩展为N轴机械臂。

作为优选，本发明计算过程的去相关和重新整合，具体以下步骤：

步骤一：输入的位姿信息

[nx ox ax px]

[ny oy ay py]

[nz oz az pz]

[0 0 0 1]

其中，x,y,z直角坐标表示机械臂末端位置，n,o,a三个旋转矢量表示机械臂末端姿态；在第I时钟计算d6*ax、d6*ay，d6表示机械臂第六根轴的长度；在I+6时钟计算mul_o1+py、mul_o2+px,其中mul_o1、mul_o2分别为第一乘法器、第二乘法器在当前时钟的输出；在I+14时钟计算add_o1/add_o2，其中 add_o1、add_o2分别为第一加法器、第二加法器在当前时钟的输出；在I+21时钟计算atan(-div_o)，其中div_o为除法器在当前时钟的输出；在I+40时钟寄存atan_o为关节角angle1，其中atan_o为反正切模块在当前时钟的输出。

步骤二：在I+40时钟计算cordic(angle1)，获得的sin(angle1)记为s1， cos(angle1)记为c1；在I+71时钟计算nx*s1、ny*c1；在I+72时钟计算nx*c1、 ny*s1；在I+73时钟计算ox*s1、oy*c1；在I+74时钟计算ox*c1、oy*s1；在 I+75时钟计算ax*s1、ay*c1；在I+76时钟计算ax*c1、ay*s1；在I+77时钟计算nyc1-nxs1，其中nyc1、nxs1分别表示ny*c1、nx*s1的乘积；在I+78时钟计算-nxc1-nys1；在I+79时钟计算oyc1-oxs1；在I+80时钟计算-oxc1-oys1；在I+81时钟计算ayc1-axs1；在I+82时钟计算-axc1-ays1；

步骤三：在I+1时钟计算d6*az；在I+7时钟计算-pz-mul_01；在I+15时钟寄存add_o1为b；在I+77时钟计算py*c1、px*s1；在I+83时钟计算pyc1-pxs1；在I+91时钟计算add_o1-d2；在I+93时钟计算d6*ax1；在I+99时钟计算 add_o1+mul_o1；在I+101时钟计算b*b；在I+107时钟寄存add_o1为a，计算 2d5*b、2a4*a，计算b*b+a4*a4，其中d5为机械臂第五轴的长度，2d5即两倍 d5的值，a4为第三第四轴的z方向平移距离，2a4为两倍a4；在I+108时钟计算2a4*b、2d5*a；在I+113时钟计算mul_o1+mul_o2；在I+114时钟计算 mul_o1-mul_o2；在I+115时钟计算mul_o1-add_o1；在I+121时钟寄存add_o1 为c_2；在I+122时钟计算add_o1/c_2；在I+123时钟计算add_o1/c_2；在I+129 时钟寄存div_o为c，计算c*c；在I+130时钟寄存div_o为d，计算d*d；在I+135时钟寄存cc；在I+136时钟计算cc-mul_o1；在I+144时钟计算add_o1+1；在I+152时钟计算add_o1*4；在I+158时钟寄存mul_o1为e；

步骤四：在I+158时钟计算sqrt(e)；在I+163时钟计算c*d；在I+169时钟计算mul_o1*2、cc*2；在I+175时钟计算sqrt(e)-2cd、2cc+2；在I+183时钟计算add_o1/add_o2；在I+190时钟寄存div_o为c23，计算mul_o1*c；在 I+196时钟计算mul_o1+d、d5*c23；在I+197时钟计算a4*c23；在I+202时钟计算a-mul_o1；在I+203时钟计算b+mul_o1；在I+204时钟寄存add_o1为s23，计算a4*add_o1；在I+205计算d5*s23；在I+210时钟计算add_o1-mul_o1；在I+211时钟计算add_o1-mul_o1；在I+218时钟计算add_o1/d5；在I+219时钟计算add_o1/-d5；在I+226时钟寄存add_o1为s2，寄存div_o为c2，计算atan(s2,c2)；在I+227时钟计算atan(s23,c23)；在I+245时钟寄存atan_o为 angle2；在I+246时钟计算atan_o-angle2寄存为angle3；；

步骤五：在I+206时钟计算nx1*s23、ny1*c23；在I+207时钟计算ox1*s23、 oy1*c23；在I+208时钟计算ax1*s23、ay1*c23；在I+209时钟计算ax1*c23、 ay1*s23；在I+212时钟计算nx1s23-ny1c23；在I+213时钟计算ox1s23-oy1c23；在I+214时钟计算ax1s23-ay1c23；在I+215时钟计算ax1c23+ay1s23；在I+220 时钟寄存add_o1为nx3；在I+221时钟寄存add_o1为ox3；在I+222时钟寄存 add_o1为ax3计算atan(ay3,ax3)；在I+223时钟寄存add_o1为az3；在I+241 时钟寄存atan_o为angle4；

步骤六：在I+241时钟计算cordic(angle4)；在I+272时钟寄存cordic_o 为s4,c4，计算ay3*s4、ax3*c4；在I+273时钟计算ox3*s4、oy3*c4；在I+274 时钟计算ny3*c4、nx3*s4；在I+278时钟计算ay3s4+ax3c4；在I+279时钟计算ox3s4-oy3c4；在I+280时钟计算ny3c4-nx3s4；在I+286时钟计算atan(s5,c5)；在I+287时钟寄存add_o为s6；在I+288时钟计算atan(s6,c6)；在I+305时钟寄存atan_o为angle5；在I+307时钟寄存atan_o为angle6；

步骤七：执行完成步骤1～6获得的angle1、angle2、angle3、angle4、angle5、angle6的值记为angle_all1，angle_all1为角度组，包含6个角度；判断angle4 是否大于0，若大于零，则angle4赋值为angle4-180°，反之赋值为 angle4+180°，然后再次执行步骤六，获得第二组angle1、angle2、angle3、 angle4、angle5、angle6的值记为angle_all2；

步骤八：修改步骤四中I+175时钟为计算-sqrt(e)-2cd，步骤一～三不变，重新执行步骤四～七，获得angle_all3和agnle_all4；

步骤九：判断angle1是否大于0，若大于零，则angle1赋值为angle1-180°，反之赋值为angle1+180°；重新执行步骤一～八，获得angle_all5、angle_all6、 angle_all7、angle_all8；此时存在八组angle1、angle2、angle3、angle4、 angle5、angle6的角度值，与前一次传感器输入角度进行比对，选取最优解。

本发明的有益效果：本发明可以减少FPGA的资源占用量。流水线复用的方法大大减少了时间延时，从而本发明实施例可以减少计算运动学方程逆解的时间成本。

附图说明

为了更清楚地说明明本发明实施例或现有技术中的技术方案，下面对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见，下面描述中的附图仅仅是本发明的一个实施例，对于本领域普通技术人员来讲，在不付出创作性劳动的前提下，还可以根据这些附图获得其他的附图。

图1是本发明实施例提供的一种六轴机械臂运动方程逆解快速求解方法的系统框图；

图2是本发明实施例提供的一种求解逆解过程的流程示意图；

具体实施方式

下面将参考附图来描述本发明所述的基于FPGA的六轴机械臂运动方程逆解快速求解的方法的实施例。显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域的普通技术人员在没有作出创造性劳动前提下所获得的的所有其他实施例，都属于本发明保护范围。

首先，需要了解的是本发明所述的六轴机械臂运动方程逆解快速求解的方法，在本实施例中是用于求解高速运动物体的运动学方程的逆解。当FPGA求解时，其数据流将连续传输进入处理器。当然，即使是低速运动的物体，也可以将其运动学轨迹进行求解。

图1是本发明的六轴机械臂运动方程逆解快速求解方法的系统框图。其中包括：

浮点乘法器，浮点除法器，浮点开方模块，Cordic模块，反正切模块，浮点加法器的输入输出之间相互通信以及主时序控制模块对所有模块的调度和通信。整个系统运行的具体步骤如下：

步骤一(S1)：通过传感器获取机械臂末端目标的运动坐标，根据求解要求目标的位置信息以及计算过程，对所有计算过程进行拆解后进行去相关，然后重新整合；

步骤二(S2)：通过FPGA直接实现逆解计算，根据重新整合后的计算过程，得到相应关节角；

步骤三(S3)：输出运动学方程的逆解所得N个角度值；

其中，本发明上述步骤一(S1)包括：

在本发明的最佳实施例中，输入的位姿信息为如下矩阵：

[nx ox ax px]

[ny oy ay py]

[nz oz az pz]

[0 0 0 1]

其中，n,o,a为三个旋转矢量，用XYZ三轴的坐标来表示该矢量。

可选的，上述位姿矩阵是可以扩展为N阶矩阵的，同样的表示的坐标轴也可以扩展为N阶，当然本实施例包括但不仅限于该矩阵。

其中，本发明上述步骤二(S2)包括：

在本发明的最佳实施例中，求解关节角的过程如图2所示，其步骤具体包括：

步骤一：在第0时钟计算d6*ax、d6*ay；在第6时钟计算mul_o1+py、 mul_o2+px；在14时钟计算add_o1/add_o2；在21时钟计算atan(div_o)；在 40时钟得到输出angle1。

步骤二：在第40时钟计算cordic(angle1)；在71时钟计算nx*s1、ny*c1；在72时钟计算nx*c1、ny*s1；在73时钟计算ox*s1、oy*c1；在74时钟计算 ox*c1、oy*s1；在75时钟计算ax*s1、ay*c1；在77时钟计算ax*c1、ay*s1；在77时钟计算nyc1-nxs1；在78时钟计算-nxc1-nys1；在79时钟计算 oyc1-oxs1；在80时钟计算-oxc1-oys1；在81时钟计算ayc1-axs1；在82时钟计算-axc1-ays1；

步骤三：在第1时钟计算d6*az；在7时钟计算-pz-mul_01；在15时钟寄存add_o1为b；在第77时钟计算py*c1、px*s1；在83时钟计算pyc1-pxs1；在91时钟计算add_o1-d2；在93时钟计算d6*ax1；在99时钟计算 add_o1+mul_o1；在101时钟计算b*b；在107时钟寄存add_o1为a，计算2d5*b、 2a4*a，计算b*b+a4*a4；在第108时钟计算2a4*b、2d5*a；在第113时钟计算mul_o1+mul_o2；在114时钟计算mul_o1-mul_o2；在115时钟计算 mul_01+add_o1；在121时钟寄存add_o1为c_2；在122时钟计算add_o1/c_2；在123时钟计算add_o1/c_2；在129时钟寄存div_o为c，计算c*c；在130 时钟寄存div_o为d，计算d*d；在135时钟寄存cc；在136时钟计算cc-mul_01；在144时钟计算add_o1+1；在152时钟计算add_o1*4；在158时钟寄存mul_o1为e。

步骤四：在第158时钟计算sqrt(e)；在163时钟计算c*d；在169时钟计算mul_01*2、cc*2；在175时钟计算sqrt(e)-2cd、2cc+2；在183时钟计算add_o1/add_o2；在190时钟寄存div_o为c23，计算mul_01*c；在196时钟计算mul_o1+d、d5*c23；在197时钟计算a4*c23；在202时钟计算a-mul_o1；在203时钟计算b+mul_o1；在204时钟寄存add_o1为s23，计算a4*add_o1；在205计算d5*s23；在210时钟计算add_o1-mul_o1；在211时钟计算 add_o1-mul_o1；在218时钟计算add_o1/d5；在219时钟计算add_o1/-d5；在226时钟寄存add_o1为s2，寄存div_o为c2，计算atan(s2,c2)；在227 时钟计算atan(s23,c23)；在245时钟寄存atan_o为angle2；在246时钟计算atan_o-angle2寄存为angle3；。

步骤五：在206时钟计算nx1*s23、ny1*c23；在207时钟计算ox1*s23、 oy1*c23；在208时钟计算ax1*s23、ay1*c23；在209时钟计算ax1*c23、ay1*s23；在212时钟计算nx1s23-ny1c23；在213时钟计算ox1s23-oy1c23；在214时钟计算ax1s23-ay1c23；在215时钟计算ax1c23+ay1s23；在220时钟寄存 add_o1为nx3；在221时钟寄存add_o1为ox3；在222时钟寄存add_o1为ax3 计算atan(ay3,ax3)；在223时钟寄存add_o1为az3；在241时钟寄存atan_o为angle4。

步骤六：在241时钟计算cordic(angle4)；在272时钟寄存cordic_o为 s4,c4，计算ay3*s4、ax3*c4；在273时钟计算ox3*s4、oy3*c4；在274时钟计算ny3*c4、nx3*s4；在278时钟计算ay3s4+ax3c4；在279时钟计算 ox3s4-oy3c4；在280时钟计算ny3c4-nx3s4；在286时钟计算atan(s5,c5)；在287时钟寄存add_o为s6；在288时钟计算atan(s6,c6)；在305时钟寄存atan_o为angle5；在307时钟寄存atan_o为angle6。

步骤七：执行完成步骤一～六获得的angle1～6的值记为angle_all1。判断angle4是否大于0，若大于零，则angle4赋值为angle4-180°，反之赋值为 angle4+180°，然后再次执行步骤六，获得第二组angle1～6的值记为angle_all2。

步骤八：修改步骤四中175时钟为计算-sqrt(e)-2cd，其余步骤不变重新执行步骤四～七，获得angle_all3和agnle_all4。

步骤九：判断angle1是否大于0，若大于零，则angle1赋值为angle1-180°，反之赋值为angle1+180°。重新执行步骤一～八，获得angle_all5～8。此时存在八组angle1～6的角度值，与前一次传感器输入角度进行比对，选取最优解。

上述，d6,d5,a4为已知的机械臂物理参数， ax,ay,az,px,py,pz,ox,oy,oz,nx,ny,nz为输入矩阵，XXX_oX为相应模块的输出(如add_o1为加法器1的输出)，且为了便于描述，在变量命名中省略了操作符，如d6ax表示d6*ax的乘积；

上述实施例仅为输入位置信息为三维，输出角度为六个的情况，该情况可以扩展为M个输出角度的情况。

上述本申请实施例中的技术方案，至少具有如下的技术效果或优点：利用流水线设计与模块时间复用相结合，仅使用2个浮点乘法器、1个浮点除法器、 1个浮点开方模块、1个Cordic模块、1个反正切模块、2个浮点加法器，全并行工作，有效降低运动学方程逆解求解所需算子的时钟延时，快速获得逆解(N 个角度值)进行输出。所以，有效解决了现有技术中计算运动学方程逆解方法存在效率低、成本高、耗时量大的技术问题，进而实现了能够高效、低成本、低耗时的求解运动学方程逆解的技术效果。

尽管已描述了本发明的优选实施例，但本领域内的技术人员一旦得知了基本创造性概念，则可对这些实施例作出另外的变更和修改。所以，所附权利要求意欲解释为包括优选实施例以及落入本发明范围的所有变更和修改。

以上所揭露的仅为本发明较佳实施例而已，当然不能以此来限定本发明之权利范围，因此依本发明权利要求所做的同等变化，仍属于本发明所涵盖的范围。

Claims

1.基于FPGA的六轴机械臂运动方程逆解求解的方法，其特征在于，FPGA系统包括两个浮点乘法器、一个浮点除法器、一个浮点开方模块、一个Cordic模块、一个反正切模块和两个浮点加法器；两个浮点乘法器、一个浮点除法器、一个浮点开方模块、一个Cordic模块、一个反正切模块和两个浮点加法器全并行工作；两个浮点乘法器包括第一浮点乘法器和第二浮点乘法器；两个浮点加法器包括第一浮点加法器和第二浮点加法器；

所述方法包括：

步骤s1、通过六轴机械臂末端目标的位姿矩阵，根据逆解求解要求目标的位置信息以及计算过程，对所有计算过程进行拆解后进行去相关，然后重新整合；

步骤s2、通过FPGA系统直接实现逆解计算，根据重新整合后的计算过程，得到相应关节角；

步骤s1和s2具体包括：

步骤一：输入的位姿信息

[nx ox ax px]

[ny oy ay py]

[nz oz az pz]

[0 0 0 1]

其中，x,y,z表示机械臂末端位置，n,o,a三个旋转矢量表示机械臂末端姿态；在第I时钟计算d6*ax、d6*ay，d6表示机械臂第六根轴的长度；在第I+6时钟计算mul_o1+py、mul_o2+px,其中mul_o1、mul_o2分别为第一浮点乘法器、第二浮点乘法器在当前时钟的输出；在第I+14时钟计算add_o1/add_o2，其中add_o1、add_o2分别为第一浮点加法器、第二浮点加法器在当前时钟的输出；在第I+21时钟计算atan(-div_o)，其中div_o为浮点除法器在当前时钟的输出；在第I+40时钟寄存atan_o为关节角angle1，其中atan_o为反正切模块在当前时钟的输出；

步骤二：在I+40时钟计算cordic(angle1)，获得的sin(angle1)记为s1，cos(angle1)记为c1；在第I+71时钟计算nx*s1、ny*c1；在第I+72时钟计算nx*c1、ny*s1；在第I+73时钟计算ox*s1、oy*c1；在第I+74时钟计算ox*c1、oy*s1；在第I+75时钟计算ax*s1、ay*c1；在第I+76时钟计算ax*c1、ay*s1；在第I+77时钟计算nyc1-nxs1，其中nyc1、nxs1分别表示ny*c1、nx*s1的乘积；在第I+78时钟计算-nxc1-nys1；在第I+79时钟计算oyc1-oxs1；在第I+80时钟计算-oxc1-oys1；在第I+81时钟计算ayc1-axs1；在第I+82时钟计算-axc1-ays1；

步骤三：在第I+1时钟计算d6*az；在第I+7时钟计算-pz-mul_o1；在第I+15时钟寄存add_o1为b；在I+77时钟计算py*c1、px*s1；在第I+83时钟计算pyc1-pxs1；在第I+91时钟计算add_o1-d2；在第I+93时钟计算d6*ax1；在第I+99时钟计算add_o1+mul_o1；在第I+101时钟计算b*b；在第I+107时钟寄存add_o1为a，计算2d5*b、2a4*a，计算b*b+a4*a4，其中d5为机械臂第五轴的长度，2d5即两倍d5的值，a4为第三第四轴的z方向平移距离，2a4为两倍a4；在第I+108时钟计算2a4*b、2d5*a；在第I+113时钟计算mul_o1+mul_o2；在第I+114时钟计算mul_o1-mul_o2；在第I+115时钟计算mul_o1-add_o1；在第I+121时钟寄存add_o1为c_2；在第I+122时钟计算add_o1/c_2；在第I+123时钟计算add_o1/c_2；在第I+129时钟寄存div_o为c，计算c*c；在第I+130时钟寄存div_o为d，计算d*d；在第I+135时钟寄存cc；在第I+136时钟计算cc-mul_o1；在第I+144时钟计算add_o1+1；在第I+152时钟计算add_o1*4；在第I+158时钟寄存mul_o1为e；

步骤四：在第I+158时钟计算sqrt(e)；在第I+163时钟计算c*d；在第I+169时钟计算mul_o1*2、cc*2；在第I+175时钟计算sqrt(e)-2cd、2cc+2；在第I+183时钟计算add_o1/add_o2；在第I+190时钟寄存div_o为c23，计算mul_o1*c；在第I+196时钟计算mul_o1+d、d5*c23；在第I+197时钟计算a4*c23；在第I+202时钟计算a-mul_o1；在第I+203时钟计算b+mul_o1；在第I+204时钟寄存add_o1为s23，计算a4*add_o1；在第I+205计算d5*s23；在第I+210时钟计算add_o1-mul_o1；在第I+211时钟计算add_o1-mul_o1；在第I+218时钟计算add_o1/d5；在第I+219时钟计算add_o1/-d5；在第I+226时钟寄存add_o1为s2在第I，寄存div_o为c2，计算atan(s2,c2)；在第I+227时钟计算atan(s23,c23)；在第I+245时钟寄存atan_o为angle2；在第I+246时钟计算atan_o-angle2寄存为angle3；

步骤五：在第I+206时钟计算nx1*s23、ny1*c23；在第I+207时钟计算ox1*s23、oy1*c23；在第I+208时钟计算ax1*s23、ay1*c23；在第I+209时钟计算ax1*c23、ay1*s23；在第I+212时钟计算nx1s23-ny1c23；在第I+213时钟计算ox1s23-oy1c23；在第I+214时钟计算ax1s23-ay1c23；在第I+215时钟计算ax1c23+ay1s23；在第I+220时钟寄存add_o1为nx3；在第I+221时钟寄存add_o1为ox3；在第I+222时钟寄存add_o1为ax3计算atan(ay3,ax3)；在第I+223时钟寄存add_o1为az3；在第I+241时钟寄存atan_o为angle4；

步骤六：在第I+241时钟计算cordic(angle4)；在第I+272时钟寄存cordic_o为s4,c4，计算ay3*s4、ax3*c4；在第I+273时钟计算ox3*s4、oy3*c4；在第I+274时钟计算ny3*c4、nx3*s4；在第I+278时钟计算ay3s4+ax3c4；在第I+279时钟计算ox3s4-oy3c4；在第I+280时钟计算ny3c4-nx3s4；在第I+286时钟计算atan(s5,c5)；在第I+287时钟寄存add_o为s6；在第I+288时钟计算atan(s6,c6)；在第I+305时钟寄存atan_o为angle5；在第I+307时钟寄存atan_o为angle6；

步骤七：执行完成步骤一～六获得的angle1、angle2、angle3、angle4、angle5、angle6的值记为angle_all1，angle_all1为角度组，包含6个角度；判断angle4是否大于0，若大于零，则angle4赋值为angle4-180°，反之赋值为angle4+180°，然后再次执行步骤六，获得第二组angle1、angle2、angle3、angle4、angle5、angle6的值记为angle_all2；

步骤九：判断angle1是否大于0，若大于零，则angle1赋值为angle1-180°，反之赋值为angle1+180°；重新执行步骤一～八，获得angle_all5、angle_all6、angle_all7、angle_all8；此时存在八组angle1、angle2、angle3、angle4、angle5、angle6的角度值，与前一次传感器输入角度进行比对，选取最优解。