倾角传感器内置调平算法的实践

本文是VALUER倾角传感器内置液压平台调平算法的实践笔记。VALUER动态倾角传感器内置的液压平台调平控制算法，主要用于基于电比例液压调平的曲臂式、直臂式高空作业车的工作平台（工作篮）的稳定控制上。由于内置调平算法，充分简化主机控制器的调平控制。开发人员简单设置一些参数，即可得到很好的平台稳定控制效果，无需自行编写PID闭环控制算法。对于双轴角度传感器，可以针对X、Y两个轴独立进行调平。

主控制器可以通过CAN总线对VALUER湾流倾角传感器进行配置，设定目标跟踪角度、跟踪灵敏度、比例阀死区电流及最大电流。VALUER角度传感器就会根据角度误差计算出每一路比例阀所需要输出的电流，并将各路比例阀电流大小数据通过CAN总线传送给主控制器。主控制器输出对应的电流驱动各路比例阀，即可完成平台稳定控制。主控制器无需再编写控制算法，从而简化了设计开发过程。该算法已经在众多企业中得到了验证，具备很强的鲁棒性，易于调节，控制效果良好，人员乘坐舒适度较高。

一、电流控制命令数据格式：PDO4【平台控制数据】

基于CAN协议的电流控制命令数据帧格式如下：

CAN_ID	DLC	B0	B1	B2	B3	B4	B5	B6	B7
0x480+NODE_ID	8	XCurrentP		XCurrentN		YCurrentP		YCurrentN

CAN_ID：

节点地址为5时，CAN_ID为0x485。

B0-B1：XcurrentP

绕X轴旋转方向正电流。按右手螺旋法则，该电流驱使平台绕X轴正方向旋转（Roll角增大方向）。

B2-B3：XcurrentN

绕X轴旋转方向负电流。按右手螺旋法则，该电流驱使平台绕X轴反方向旋转（Roll角减小方向）。

B4-B5：YcurrentP

绕Y轴旋转方向正电流。按右手螺旋法则，该电流驱使平台绕Y轴正方向旋转（Pitch角增大方向）。

B6-B7：YcurrentN

绕Y轴旋转方向负电流。按右手螺旋法则，该电流驱使平台绕Y轴反方向旋转（Pitch角减小方向）。

二、参数配置数据格式

以下是CANopen数据格式下的SDO命令索引。

Index 主索引	SubIndex 子索引	R/W 读/写	Name 参数	Value（Data0 ~ Data3） 值
0x6002 参 数 配 置 主 索 引	0x00	只写	写参数使能	Data0： 0：写参数禁止。为了数据安全，在不进行写操作时，请务必退出写参数使能模式。 1：写参数使能。只有在写参数使能时，才能对主索引0x6002下的各个子索引值进行写操作。
	0x22	只写	坐标系方向配置	Data0： 0x01：1#坐标系 0x02：2# 坐标系 0x03：3#坐标系 0x04：4#坐标系 0x05：5#坐标系 0x06：6#坐标系 Data1-Data2： 代表绕Z轴旋转角度，为16-bit有符号整数，表示范围为 -180° ~ +180°。缩放系数：0.1
	0x23	只写
	0x24	只写	ROLL角范围	Data0： 0x00：-180°~ +180° 0x01：0° ~ 360° 0x02：多圈测量 Data1： 0xF0：保存到EEPROM other：不保存
	0x70	只写	Roll角调节参数之： 【目标角度，灵敏度】	Data0~Data1: 缩放系数0.1，表示范围±180° Data2: 正向灵敏度，0-100 Data3: 反向灵敏度，0-100
	0x71		Roll角调节参数之： 【死区电流】	Data0~Data1:【正向死区电流】 表示范围0 – 3000 mA Data2~Data3:【反向死区电流】 表示范围0 – 3000 mA
	0x72		Roll角调节参数之： 【最大电流】	Data0~Data1:【正向最大电流】 表示范围0 – 3000 mA Data2~Data3:【反向最大电流】 表示范围0 – 3000 mA
	0x73
	0x78	只写	Pitch角调节参数之： 【目标角度】	Data0~Data1: 缩放系数0.1，表示范围±90° Data2: 正向灵敏度，0-100 Data3: 反向灵敏度，0-100
	0x79		Pitch角调节参数之： 【死区电流】	Data0~Data1:【正向死区电流】 表示范围0 – 3000 mA Data2~Data3:【反向死区电流】 表示范围0 – 3000 mA
	0x7A		Pitch角调节参数之： 【最大电流】	Data0~Data1:【正向最大电流】 表示范围0 – 3000 mA Data2~Data3:【反向最大电流】 表示范围0 – 3000 mA
	0x7B
	0x7F		Roll，Pitch角调节参数保存	Data3: 0xF0：保存到EEPROM。请不要频繁刷写EEPROM。EEPROM刷写寿命为100万次。 other：不保存。
	0xFF	只写

三、参数说明

1、目标角度

目标角度，也就是平台跟踪角度。出厂默认为0°。对于双轴角度传感器，有两个目标跟踪角度，一个是X目标角，一个Y目标角。特别注意，X目标角对应实际控制角Roll角，Y目标角对应的实际控制角为Pitch角。

Roll角测量范围为±180°。Pitch角测量范围为±90°。相应的每一个轴的目标角度设置时不能超出这个范围。在数据格式中，目标角度为有符号16位数，共两个字节，低字节在前，高字节在后，缩放系数为0.1。

2、灵敏度

调节电流有正向和负向之分。灵敏度对应的是控制的灵敏度。灵敏度设置范围在0 – 100之间对应0-100%。灵敏度为无符号8位数，共一个字节。出厂默认灵敏度为30。灵敏度越高，对应调节越刚硬，灵敏度越低，对应调节越柔和。过高的灵敏度有可能导致平台振荡。过低的灵敏度会导致跟踪误差较大。

3、死区电流

死区电流也有正向和负向之分。死区电流为比例阀驱动油缸或马达动作的最小电流。死区电流为无符号16位数，共两个字节，低字节在前，高字节在后。表示范围为0 – 3000 对应 0-3000mA电流。

上图为一电磁比例阀输入电流与流量特性曲线图。死区电流大概在300mA位置。实际死区电流需实车进行测试得到。一般的测试方法为，给一恒定电流（比如300mA），工作装置是否动作。如果未动作则增加电流，如果动作了就减小电流。直到找到一个电流值，使得工作装置刚好有肉眼所能察觉的缓慢动作为止。这个电流值就是死区电流。为提高低速时的调节性能，死区电流取值也可以比该电流值大2-5mA。

​​​​​​​4、最大电流

最大电流也有正向和负向之分。最大电流为比例阀驱动油缸或马达动作的最大有效电流。最大电流为无符号16位数，共两个字节，低字节在前，高字节在后。表示范围为0 – 3000 对应 0-3000mA电流。从上图电磁比例阀输入电流与流量特性曲线可知，最大电流为500mA左右。

在实际液压系统中，有时最大电流500mA是一个非常巨大的值。500mA的电流对应的工作台的旋转速度可能非常快，可能达到了20 °每秒。也许400mA时工作台旋转速度可能为10°每秒，这个速度或许已经绝对满足调节要求了。那么用户可以将最大电流设置为400mA。

​​​​​​​四、完整设置应用例举

以下给出完整的通过CAN总线进行平台控制参数配置的操作过程，例举如下：

时序	数据源	数据帧(16进制)	功能
1	主站	605,23 02 60 00 01 00 00 00	写参数使能，允许写入参数
2	主站	605,23 02 60 71 FA 00 2C 01	X轴死区电流设置 正向死区电流250mA，十进制250，十六进制0x00FA 负向死区电流300mA，十进制300，十六进制0x012C
3	主站	605, 23 02 60 79 C8 00 5E 01	Y轴死区电流设置 正向死区电流200mA，十进制200，十六进制0x00C8 负向死区电流350mA，十进制350，十六进制0x015E
4	主站	605,23 02 60 72 F4 01 26 02	X轴最大电流设置 正向最大电流500mA，十进制500，十六进制0x01F4 负向最大电流550mA，十进制550，十六进制0x0226
5	主站	605, 23 02 60 7A 58 02 8A 02	Y轴最大电流设置 正向最大电流600mA，十进制600，十六进制0x0258 负向最大电流650mA，十进制650，十六进制0x028A
6	主站	605,23 02 60 70 00 00 1E 28	X轴目标角度、灵敏度设置 目标角度设为0°,十六进制0x0000 正向灵敏度设为30%，十进制30，十六进制0x1E 负向灵敏度设为40%，十进制40，十六进制0x28
7	主站	605, 23 02 60 78 7C FC 32 3C	Y轴目标角度、灵敏度设置 目标角度设为-90°，缩放系数0.1。十进制-900，十六进制0xFC7C 正向灵敏度设为50%，十进制50，十六进制0x32 负向灵敏度设为60%，十进制60，十六进制0x3C
8	主站	605, 23 02 60 7F 00 00 00 F0	保存所有控制参数。 最后一个字节如果不为F0，则不保存参数。

说明：

第1步：在发送参数设置命令之前，一定要先确保写参数使能。可通过写参数使能命令来打开写参数使能。

第2-7步：死区电流、最大电流、目标角度、灵敏度设置。命令执行无先后顺序，可按需设置。

第8步：待所有命令设置完毕后，可发送保存参数命令。请不要频繁发送参数保存命令，以免损坏存储器。

用户可以通过VALUER桌面端软件来生成对应CAN数据格式，以帮助快速设置这些参数，以下是软件界面图。