CN104571116A - 一种光电稳定平台的位置回路坐标系转换方法 - Google Patents

Abstract

本发明提出一种光电稳定平台位置回路坐标系转换方法，适用于采用旋转变压器或绝对式光电编码器等角位置传感器的有限转角电机传动装置。本发明利用光电稳定平台自身的速度回路，分时对负载进行正程、逆程恒速驱动，在转动过程中实时监控负载的运动速率，并和阈值进行比较。在满足阈值条件下，分别读取位置回路左机械限位、右机械限位和近似机械中点的位置传感器原码，然后计算角位置传感器原坐标系与稳定平台坐标系的极性差；进而通过计算将角位置传感器坐标系映射到稳定平台坐标系。该方法完全由软件自动实现，不增加硬件成本开销，并且可以消除角位置传感器与机械旋转轴的安装偏差，解决了以往对安装误差要求严格、人工参与调试效率低下的问题。

Description

一种光电稳定平台的位置回路坐标系转换方法

技术领域

本发明属于伺服控制领域，主要涉及一种光电稳定平台位置回路坐标系转换方法，尤其涉及一种采用旋转变压器或绝对式光电编码器作为角位置传感器的光电稳定平台位置回路坐标系转换方法。

背景技术

光电稳定平台是一种安装在武装载体上，用于对目标进行搜索、瞄准、打击的光电系统。为了隔离载体扰动，稳定瞄准线，光电平台的控制系统通常由电流回路、速度回路和位置回路按照顺序自内而外嵌套组成多回路复合控制，以实现更高的稳定精度。位置回路的主要作用是响应操作手操控指令，精确控制瞄准线在平台坐标系下的位置指向。

位置回路的主要反馈元件是角位置传感器，如旋转式可变电位计、旋转变压器、光电编码器等。由于角位置传感器出厂时已有自身固有的坐标系，要想直接使用传感器原始值来表征位置回路的当前位置，必须要求角位置传感器的坐标系零点和极性与光电稳定平台的位置回路保持一致，在装配时需要反复读取原始值、校正、重新装配，效率非常低下。申请号为201010584293.4的专利公开了一种有限转角位置回路的位置坐标确定方法，其原理是：采用增量式码盘、电流检测电路和光电开关组合定位，当I/O口检测到光电开关TTL信号或到达正向机械限位时反向步进，直到光电开关信号被检测到或脉冲计数器达到预设的偏角时即认为找到系统零点。这种方法有两个缺陷：一是增加了检测电路、光电开关等硬件；二是光电开关作为一种实物器件，其物理宽度可导致TTL信号在一个很宽的角度范围内均可以被检测到，精度无法保证。申请号为201210288976.4的专利公开了一种位置传感器系统的调节方法，其原理是用计算机随时监测旋转变压器原始值，将旋转变压器转子随电机转动一角度后解除联动，然后再用拨叉手动旋转变压器转子，使得旋转变压器原码匹配电机旋转位置。这种方法的缺陷是，使用机械装置和人工操作，效率低下；其采用的手动拨叉装置也不适合在某些集成度高、内部空间狭小的系统中使用。

发明内容

为解决现有技术存在的问题，本发明提出了一种光电稳定平台的位置回路坐标系转换方法，该方法无需增加硬件成本和人工参与，并且不依赖于位置传感器装配精度和极性。

本发明的技术方案为：

所述一种光电稳定平台的位置回路坐标系转换方法，其特征在于：采用以下步骤：

步骤1：初始化变量：置计数器I_counter＝0，位置传感器极性P＝0，速度指令Cmd＝0，全程运动时间变量I_time＝0；

步骤2：打开速度回路，由以下公式计算当前速度回路的速度指令Cmd：

$\mathrm{Cmd}=\frac{\mathrm{\ΔR}}{t*V_{\max }}*C_{\max }$

式中，ΔR是负载的机械转动角度；t是负载的期望运动时间，V_max是速度回路的最大速度量，C_max是最大速度量对应的速度指令，t的取值使Cmd＜0.2*C_max；

步骤3：根据Cmd的值对速度阈值V_d进行估算：

V_d＝K_i*Cmd/C_max*V_max

式中，K_i是比例因子，取值范围为0.1至0.4；

步骤4：以速度指令Cmd驱动速度回路，使得负载正向恒速转动，并实时采集速度反馈信号V，并与阈值V_d比较，当|V|＜V_d时，读取正向机械限位的位置传感器原码C_r2d；

步骤5：以速度指令-Cmd驱动速度回路，使得负载逆向恒速转动，此时启动计数器I_counter统计负载转动的时间；实时采集速度反馈信号V，当|V|＜V_d时，将I_counter当前值赋给I_time，并置I_counter＝0，同时读取逆向机械限位的位置传感器原码为A_r2d；

步骤6：以速度指令Cmd驱动速度回路，使得负载正向恒速转动，启动计数器I_counter统计负载转动的时间，当时，记录近似机械中点的位置传感器原码为B_r2d；

步骤7：计算位置回路所关联的角位置传感器的数据变化极性：

取D_cb＝C_r2d-B_r2d，D_ba＝B_r2d-A_r2d；

如果|D_cb|≥|D_ba|且D_ba>0，P＝1；

如果|D_cb|＞|D_ba|且D_ba<0，P＝-1；

如果|D_cb|＜|D_ba|且D_cb>0，P＝1；

如果|D_cb|＜|D_ba|且D_cb<0，P＝-1；

P＝1时，表明角位置传感器数据变化的极性与位置回路坐标系极性定义相同，P＝-1时，表明角位置传感器数据变化的极性与位置回路坐标系极性定义相反；

步骤8：通过以下方法归一化ΔD：

取D_ca＝C_r2d-A_r2d

当P＝1且D_ca>0时：ΔD＝D_ca

当P＝1且D_ca<0时：ΔD＝D_ca+R2D_max

当P＝-1且D_ca>0时：ΔD＝R2D_max-D_ca

当P＝-1且D_ca<0时：ΔD＝-D_ca

其中R2D_max是位置传感器的满量程值对应的原码；ΔD是实际测量的负载机械转动范围对应的归一化原码值；

步骤9：通过以下转换过程得到负载在机械转动范围内任意位置的角位置传感器原码X_r2d在位置回路坐标系下的角度值Angle：

取D_xa＝X_r2d-A_r2d

当P＝1且D_xa>0时： $\mathrm{Angle}=\frac{(2*D_{\mathrm{xa}}-\mathrm{\&Delta;D})\&times;360}{2*{R2D}_{\max }}$

当P＝1且D_xa<0时： $\mathrm{Angle}=\frac{(2*D_{\mathrm{xa}}+2*{R2D}_{\max }-\mathrm{\&Delta;D})\&times;360}{2*{R2D}_{\max }}$

当P＝-1且D_xa>0时： $\mathrm{Angle}=\frac{(2*{R2D}_{\max }-2*D_{\mathrm{xa}}-\mathrm{\&Delta;D})\&times;360}{2*{R2D}_{\max }}$

当P＝-1且D_xa<0时： $\mathrm{Angle}=\frac{(-2*D_{\mathrm{xa}}-\mathrm{\&Delta;D})\&times;360}{2*{R2D}_{\max }}.$

有益效果

本发明的整体技术效果体现在以下几个方面：

(一)本发明所述方法可作为软件模块运行在光电稳定平台的CPU处理单元，不额外增加硬件电路和成本；

(二)本发明所述方法不依赖于位置传感器的安装方向和精度，简化了机械装调步骤；

(三)本发明完全由程序自动完成特征点采样和坐标转换，不需要其它辅助设备，大大提高了调试和外场维修时的方便性和效率；

(四)本发明充分利用了光电稳定平台自身速度回路闭环控制高精度、高分辨率以及软件周期定时触发的特点，采用速度反馈和软件时间计数的方法，在判断采样位置时精确可靠，比人工调整具有更高的一致性和精度；

(五)该方法完全由软件实现，可移植性好，其思路可适用于其他采用旋变或绝对式光电码盘作为角位置传感器的机电控制系统，尤其适用于结构紧凑、空间狭小、装配难度大，不适合采用辅助设备进行校正装配的控制系统。

附图说明

图1是本发明所述坐标系变换方法的流程图；

图2是位置传感器与位置回路坐标系相对关系的示意图；

图3是实施实例内俯仰位置回路坐标系转换结果；

图4是实施实例内方位位置回路坐标系转换结果；

图5是实施实例外俯仰位置回路坐标系转换结果；

图6是实施实例外方位位置回路坐标系转换结果。

具体实施方式

下面结合具体实施例描述本发明：

本发明的优选实例是某两轴四框架光电稳定平台，四个机电转动机构分别是内俯仰(ie)、内方位(ia)、外俯仰(oe)、外方位(oa)，均采用16位精度的旋转变压器作为位置传感器，该旋转变压器0-360°对应的原码为0-65535。一个任意安装的旋转变压器，其原码与对应位置回路坐标系的相对关系可分为六种情况，如附图2所示。其中，外圈表示位置回路的坐标定义，内圈表示旋转变压器原码，A,B,C分别表示机械转轴的左机械限位、近似中点和右机械限位，通常定义C为位置回路的正向，A为负向。P表示位置传感器数据变化极性，J表示旋转变压器的零位，即0-360°跳跃点。

为了解决旋变安装的极性和偏差带来的位置回路坐标系转换问题，采用本发明所述方法进行软件校正，采用该方法的软件模块已经作为伺服控制软件的一部分存储于光电稳定平台的CPU处理单元。调试人员通过串口连接计算机板，给系统通电后在超级终端运行软件模块入口函数，即可自动完成坐标转换过程。

下面，分别以四个机电转动机构的坐标系变换过程作为实施实例，进一步详述本发明。

实施实例一：内俯仰(ie)

步骤1：初始化变量：置计数器I_counter＝0，位置传感器极性P＝0，速度指令Cmd＝0，全程运动时间变量I_time＝0；

步骤2：打开速度回路，由以下公式计算内俯仰的速度回路速度指令Cmd：

$\mathrm{Cmd}=\frac{\mathrm{\&Delta;R}}{t*V_{\max }}*C_{\max }$

式中，ΔR是负载的机械转动角度；t是负载的期望运动时间，V_max是速度回路的最大速度量，C_max是最大速度量对应的速度指令，ΔR、V_max、C_max都是已知量，t的取值使Cmd＜0.2*C_max，这里设置为4秒；则内俯仰速度回路的速度指令为：

步骤3：根据Cmd的值对速度阈值V_d进行估算：

V_d＝K_i*Cmd/C_max*V_max

式中，K_i是比例因子，取值范围为0.1至0.4；这里取K_i＝0.2，则内俯仰阈值分别为：

V_d＝0.2*Cmd/10*60°/s＝0.75°/s

步骤4：以速度指令Cmd驱动内俯仰速度回路，使得负载正向恒速转动，并实时采集速度反馈信号V，并与阈值V_d比较，当|V|＜V_d时，特征位置C的旋转变压器原始值C_r2d读取结果为：C_r2d＝812；

步骤5：以速度指令-Cmd驱动内俯仰速度回路，使得负载逆向恒速转动，此时启动计数器I_counter统计负载转动的时间；实时采集速度反馈信号V，当|V|＜V_d时，将I_counter当前值赋给I_time，并置I_counter＝0，同时读取该特征位置A的旋转变压器原始值为A_r2d；结果为：I_time＝799，A_r2d＝63616；

步骤6：以速度指令Cmd驱动内俯仰速度回路，使得负载正向恒速转动，启动计数器I_counter统计负载转动的时间，当时，特征位置B的旋转变压器原始值B_r2d读取结果为：B_r2d＝64981；

步骤7：计算旋转变压器的数据变化极性：

D_cb＝C_r2d-B_r2d＝812-64981＝-64169

D_ba＝B_r2d-A_r2d＝64981-63616＝1365

由于|D_cb|≥|D_ba|且D_ba>0，因此P＝1；表明角位置传感器数据变化的极性与位置回路坐标系极性定义相同；

步骤8：通过以下方法归一化ΔD：

D_ca＝C_r2d-A_r2d＝812-63616＝-62804

由于P＝1且D_ca<0，因此ΔD＝D_ca+R2D_max＝-62804+65535＝2731

其中R2D_max是位置传感器的满量程值对应的原码；ΔD是实际测量的负载机械转动范对应的归一化原码值；

步骤9：通过以下转换过程得到负载在机械转动范围内任意位置的旋转变压器原码X_r2d在内俯仰位置回路坐标系下的角度值Angle：

取D_xa＝X_r2d-A_r2d

当P＝1且D_xa>0时： $\mathrm{Angle}=\frac{(2*D_{\mathrm{xa}}-\mathrm{\&Delta;D})\&times;360}{2*{R2D}_{\max }}$

当P＝1且D_xa<0时： $\mathrm{Angle}=\frac{(2*D_{\mathrm{xa}}+2*{R2D}_{\max }-\mathrm{\&Delta;D})\&times;360}{2*{R2D}_{\max }}$

内俯仰旋转变压器与位置回路的相对关系和坐标系转换结果见附图3。

实施实例二：内方位(ia)

步骤1：初始化变量：置计数器I_counter＝0，位置传感器极性P＝0，速度指令Cmd＝0，全程运动时间变量I_time＝0；

步骤2：打开速度回路，由以下公式计算内方位的速度回路速度指令Cmd：

$\mathrm{Cmd}=\frac{\mathrm{\&Delta;R}}{t*V_{\max }}*C_{\max }$

式中，ΔR是负载的机械转动角度；t是负载的期望运动时间，V_max是速度回路的最大速度量，C_max是最大速度量对应的速度指令，ΔR、V_max、C_max都是已知量，t的取值使Cmd＜0.2*C_max，这里设置为4秒；则内方位速度回路的速度指令为：

步骤3：根据Cmd的值对速度阈值V_d进行估算：

V_d＝K_i*Cmd/C_max*V_max

式中，K_i是比例因子，取值范围为0.1至0.4；这里取K_i＝0.2，则内方位阈值分别为：

V_d＝0.2*Cmd/10*60°/s＝0.6°/s

步骤4：以速度指令Cmd驱动内方位速度回路，使得负载正向恒速转动，并实时采集速度反馈信号V，并与阈值V_d比较，当|V|＜V_d时，特征位置C的旋转变压器原始值C_r2d读取结果为：C_r2d＝34538；

步骤5：以速度指令-Cmd驱动内方位速度回路，使得负载逆向恒速转动，此时启动计数器I_counter统计负载转动的时间；实时采集速度反馈信号V，当|V|＜V_d时，将I_counter当前值赋给I_time，并置I_counter＝0，同时读取该特征位置A的旋转变压器原始值为A_r2d；结果为：I_time＝800，A_r2d＝32354；

步骤6：以速度指令Cmd驱动内方位速度回路，使得负载正向恒速转动，启动计数器I_counter统计负载转动的时间，当时，特征位置B的旋转变压器原始值B_r2d读取结果为：B_r2d＝33446；

步骤7：计算旋转变压器的数据变化极性：

D_cb＝C_r2d-B_r2d＝34538-33446＝1092

D_ba＝B_r2d-A_r2d＝33446-32354＝1092

由于|D_cb|≥|D_ba|且D_ba>0，因此P＝1；表明角位置传感器数据变化的极性与位置回路坐标系极性定义相同；

步骤8：通过以下方法归一化ΔD：

D_ca＝C_r2d-A_r2d＝34538-32354＝2184

由于P＝1且D_ca>0，因此ΔD＝D_ca＝2184

其中R2D_max是位置传感器的满量程值对应的原码；ΔD是实际测量的负载机械转动范围对应的归一化原码值；

步骤9：通过以下转换过程得到负载在机械转动范围内任意位置的旋转变压器原码X_r2d在内方位位置回路坐标系下的角度值Angle：

取D_xa＝X_r2d-A_r2d

当P＝1且D_xa>0时： $\mathrm{Angle}=\frac{(2*D_{\mathrm{xa}}-\mathrm{\&Delta;D})\&times;360}{2*{R2D}_{\max }}$

当P＝1且D_xa<0时： $\mathrm{Angle}=\frac{(2*D_{\mathrm{xa}}+2*{R2D}_{\max }-\mathrm{\&Delta;D})\&times;360}{2*{R2D}_{\max }}$

内方位旋转变压器与位置回路的相对关系和坐标系转换结果见附图4。

实施实例三：外俯仰(oe)

步骤1：初始化变量：置计数器I_counter＝0，位置传感器极性P＝0，速度指令Cmd＝0，全程运动时间变量I_time＝0；

步骤2：打开速度回路，由以下公式计算外俯仰的速度回路速度指令Cmd：

$\mathrm{Cmd}=\frac{\mathrm{\&Delta;R}}{t*V_{\max }}*C_{\max }$

式中，ΔR是负载的机械转动角度；t是负载的期望运动时间，V_max是速度回路的最大速度量，C_max是最大速度量对应的速度指令，ΔR、V_max、C_max都是已知量，t的取值使Cmd＜0.2*C_max，这里设置为20秒；则外俯仰速度回路的速度指令为：

步骤3：根据Cmd的值对速度阈值V_d进行估算：

V_d＝K_i*Cmd/C_max*V_max

式中，K_i是比例因子，取值范围为0.1至0.4；这里取K_i＝0.2，则外俯仰阈值分别为：

V_d＝0.2*Cmd/10*60°/s＝1.3°/s

步骤4：以速度指令Cmd驱动外俯仰速度回路，使得负载正向恒速转动，并实时采集速度反馈信号V，并与阈值V_d比较，当|V|＜V_d时，特征位置C的旋转变压器原始值C_r2d读取结果为：C_r2d＝55449；

步骤5：以速度指令-Cmd驱动外俯仰速度回路，使得负载逆向恒速转动，此时启动计数器I_counter统计负载转动的时间；实时采集速度反馈信号V，当|V|＜V_d时，将I_counter当前值赋给I_time，并置I_counter＝0，同时读取该特征位置A的旋转变压器原始值为A_r2d；结果为：I_time＝4000，A_r2d＝13580；

步骤6：以速度指令Cmd驱动外俯仰速度回路，使得负载正向恒速转动，启动计数器I_counter统计负载转动的时间，当时，特征位置B的旋转变压器原始值B_r2d读取结果为：B_r2d＝1747；

步骤7：计算旋转变压器的数据变化极性：

D_cb＝C_r2d-B_r2d＝55449-1747＝53669

D_ba＝B_r2d-A_r2d＝1747-13580＝-11833

由于|D_cb|≥|D_ba|且D_ba<0，因此P＝-1；表明角位置传感器数据变化的极性与位置回路坐标系极性定义相反；

步骤8：通过以下方法归一化ΔD：

D_ca＝C_r2d-A_r2d＝55449-13580＝41869

由于P＝-1且D_ca>0，因此ΔD＝R2D_max-D_ca＝23666

其中R2D_max是位置传感器的满量程值对应的原码；ΔD是实际测量的负载机械转动范围对应的归一化原码值；

步骤9：通过以下转换过程得到负载在机械转动范围内任意位置的旋转变压器原码X_r2d在外俯仰位置回路坐标系下的角度值Angle：

取D_xa＝X_r2d-A_r2d

当P＝-1且D_xa>0时： $\mathrm{Angle}=\frac{(2*{R2D}_{\max }-2*D_{\mathrm{xa}}-\mathrm{\&Delta;D})\&times;360}{2*{R2D}_{\max }}$

当P＝-1且D_xa<0时： $\mathrm{Angle}=\frac{(-2*D_{\mathrm{xa}}-\mathrm{\&Delta;D})\&times;360}{2*{R2D}_{\max }}$

由于实际需要，外俯仰位置回路的零点定义并不在机械旋转中心，将转换后的坐标系加上偏移量，转换后外俯仰旋转变压器与位置回路的相对关系和坐标系转换结果见附图5。

实施实例四：外方位(oa)

步骤1：初始化变量：置计数器I_counter＝0，位置传感器极性P＝0，速度指令Cmd＝0，全程运动时间变量I_time＝0；

步骤2：打开速度回路，由以下公式计算外方位的速度回路速度指令Cmd：

$\mathrm{Cmd}=\frac{\mathrm{\&Delta;R}}{t*V_{\max }}*C_{\max }$

式中，ΔR是负载的机械转动角度；t是负载的期望运动时间，V_max是速度回路的最大速度量，C_max是最大速度量对应的速度指令，ΔR、V_max、C_max都是已知量，t的取值使Cmd＜0.2*C_max，这里设置为20秒；则外方位速度回路的速度指令为：

步骤3：根据Cmd的值对速度阈值V_d进行估算：

V_d＝K_i*Cmd/C_max*V_max

式中，K_i是比例因子，取值范围为0.1至0.4；这里取K_i＝0.2，则外方位阈值分别为：

V_d＝0.2*Cmd/10*60°/s＝2.55°/s

步骤4：以速度指令Cmd驱动外方位速度回路，使得负载正向恒速转动，并实时采集速度反馈信号V，并与阈值V_d比较，当|V|＜V_d时，特征位置C的旋转变压器原始值C_r2d读取结果为：C_r2d＝19506；

步骤5：以速度指令-Cmd驱动外方位速度回路，使得负载逆向恒速转动，此时启动计数器I_counter统计负载转动的时间；实时采集速度反馈信号V，当|V|＜V_d时，将I_counter当前值赋给I_time，并置I_counter＝0，同时读取该特征位置A的旋转变压器原始值为A_r2d；结果为：I_time＝4000，A_r2d＝392；

步骤6：以速度指令Cmd驱动外方位速度回路，使得负载正向恒速转动，启动计数器I_counter统计负载转动的时间，当时，特征位置B的旋转变压器原始值B_r2d读取结果为：B_r2d＝42716；

步骤7：计算旋转变压器的数据变化极性：

D_cb＝C_r2d-B_r2d＝19506-42716＝-23210

D_ba＝B_r2d-A_r2d＝42716-392＝42324

由于|D_cb|<|D_ba|且D_cb<0，因此P＝-1；表明角位置传感器数据变化的极性与位置回路坐标系极性定义相反；

步骤8：通过以下方法归一化ΔD：

D_ca＝C_r2d-A_r2d＝19506-392＝19114

由于P＝-1且D_ca>0，因此ΔD＝R2D_max-D_ca＝46421

其中R2D_max是位置传感器的满量程值对应的原码；ΔD是实际测量的负载机械转动范围对应的归一化原码值；

步骤9：通过以下转换过程得到负载在机械转动范围内任意位置的旋转变压器原码X_r2d在外方位位置回路坐标系下的角度值Angle：

取D_xa＝X_r2d-A_r2d

当P＝-1且D_xa>0时： $\mathrm{Angle}=\frac{(2*{R2D}_{\max }-2*D_{\mathrm{xa}}-\mathrm{\&Delta;D})\&times;360}{2*{R2D}_{\max }}$

当P＝-1且D_xa<0时： $\mathrm{Angle}=\frac{(-2*D_{\mathrm{xa}}-\mathrm{\&Delta;D})\&times;360}{2*{R2D}_{\max }}$

外方位旋转变压器与位置回路的相对关系和坐标系转换结果见附图6。

Claims (1)

1.一种光电稳定平台的位置回路坐标系转换方法，其特征在于：采用以下步骤：

步骤1：初始化变量：置计数器I_counter＝0，位置传感器极性P＝0，速度指令Cmd＝0，全程运动时间变量I_time＝0；

步骤2：打开速度回路，由以下公式计算当前速度回路的速度指令Cmd：

$\mathrm{Cmd}=\frac{\mathrm{\&Delta;R}}{t*V_{\max }}*C_{\max }$

式中，ΔR是负载的机械回转角度；t是负载的期望运动时间，V_max是速度回路的最大速度量，C_max是最大速度量对应的速度指令，t的取值使Cmd＜0.2*C_max；

步骤3：根据Cmd的值对速度阈值V_d进行估算：

V_d＝K_i*Cmd/C_max*V_max

式中，K_i是比例因子，取值范围为0.1至0.4；

步骤4：以速度指令Cmd驱动速度回路，使得负载正向恒速转动，并实时采集速度反馈信号V，并与阈值V_d比较，当|V|＜V_d时，读取正向机械限位的位置传感器原码C_r2d；

步骤5：以速度指令-Cmd驱动速度回路，使得负载逆向恒速转动，此时启动计数器I_counter统计负载转动的时间；实时采集速度反馈信号V，当|V|＜V_d时，将I_counter当前值赋给I_time，并置I_counter＝0，同时读取逆向机械限位的位置传感器原码为A_r2d；

步骤6：以速度指令Cmd驱动速度回路，使得负载正向恒速转动，启动计数器I_counter统计负载转动的时间，当时，记录近似机械中点的位置传感器原码为B_r2d；

步骤7：计算位置回路所关联的角位置传感器的数据变化极性：

取D_cb＝C_r2d-B_r2d，D_ba＝B_r2d-A_r2d；

如果|D_cb|≥|D_ba|且D_ba>0，P＝1；

如果|D_cb|＞|D_ba|且D_ba<0，P＝-1；

如果|D_cb|＜|D_ba|且D_cb>0，P＝1；

如果|D_cb|＜|D_ba|且D_cb<0，P＝-1；

P＝1时，表明角位置传感器数据变化的极性与位置回路坐标系极性定义相同，P＝-1时，表明角位置传感器数据变化的极性与位置回路坐标系极性定义相反；

步骤8：通过以下方法归一化ΔD：

取D_ca＝C_r2d-A_r2d

当P＝1且D_ca>0时：ΔD＝D_ca

当P＝1且D_ca<0时：ΔD＝D_ca+R2D_max

当P＝-1且D_ca>0时：ΔD＝R2D_max-D_ca

当P＝-1且D_ca<0时：ΔD＝-D_ca

其中R2D_max是位置传感器的满量程值对应的原码；ΔD是实际测量的负载机械转动范围对应的归一化原码值；

步骤9：通过以下转换过程得到负载在机械转动范围内任意位置的角位置传感器原码X_r2d在位置回路坐标系下的角度值Angle：

取D_xa＝X_r2d-A_r2d

当P＝1且D_xa>0时： $\mathrm{Angle}=\frac{(2*D_{\mathrm{xa}}-\mathrm{\&Delta;D})\&times;360}{2*R2D_{\max }}$

当P＝1且D_xa<0时： $\mathrm{Angle}=\frac{(2*D_{\mathrm{xa}}+2*R2D_{\max }-\mathrm{\&Delta;D})\&times;360}{2*R2D_{\max }}$

当P＝-1且D_xa>0时： $\mathrm{Agnle}=\frac{(2*R2D_{\max }-2*D_{\mathrm{xa}}-\mathrm{\&Delta;D})\&times;360}{2*R2D_{\max }}$

当P＝-1且D_xa<0时： $\mathrm{Angle}=\frac{(-2*D_{\mathrm{xa}}-\mathrm{\&Delta;D})\&times;360}{2*R2D_{\max }}$

。