自动引导车的主动响应测量装置及控制方法与流程

1.本发明涉及自动引导车性能检测技术领域，尤其是涉及一种能够使安装在自动引导车上的被测靶球主动响应激光测量设备的测试光源，使激光测量设备能够实时测量被测自动引导车的空间位姿数据的自动引导车的主动响应测量装置及控制方法。


背景技术：

2.近年来，我国自动引导车 (automated guided vehicle，agv)产业得到了快速发展。
3.现有agv种类繁多不容易辨别其性能优劣，因为一般的被测靶球受光面角度较小，在agv平移较大距离或旋转较大角度时，安装在 agv上的被测靶球的受光面会偏离激光测量设备的测试光源，使激光测量设备无法对被测靶球进行测量。


技术实现要素：

4.本发明的发明目的是为了克服现有技术中安装在自动引导车上的被测靶球的受光面会偏离激光测量设备的测试光源，使激光测量设备无法对被测靶球进行测量的不足，提供了一种能够使安装在自动引导车上的被测靶球主动响应激光测量设备的测试光源，使激光测量设备能够实时测量被测自动引导车的空间位姿数据的自动引导车的主动响应测量装置及控制方法。
5.为了实现上述目的，本发明采用以下技术方案：
6.一种自动引导车的主动响应测量装置，包括计算机，无线信号发射器，激光跟踪仪，设于自动引导车上的盒体，设于盒体中的数据处理器、姿态传感器、电机驱动器、无线信号接收器和电机，设于盒体上方的靶球座和设于靶球座上的被测靶球；数据处理器分别与无线信号接收器、姿态传感器和电机驱动器数据连接，电机驱动器与电机数据连接，电机的转轴上端伸出盒体之外并与靶球座连接，计算机分别与无线信号发射器和激光跟踪仪数据连接，无线信号发射器和无线信号接收器无线连接，被测靶球位于激光跟踪仪的斜下方。
7.作为优选，所述自动引导车为扫地机器人或叉车；所述姿态传感器为陀螺仪。
8.一种自动引导车的主动响应测量装置的控制方法，包括如下步骤：
9.3-1，激光跟踪仪测量被测靶球的位置数据p(a,z,d)；计算机控制无线信号发射器将位置数据p(a,z,d)中的a发送给无线信号接收器；
10.其中，a为在激光跟踪仪的测量坐标系下被测靶球相对于测量坐标系x轴的偏移角度；z为测量坐标系下被测靶球相对于测量坐标系 z轴正方向的夹角；d为被测靶球与测量坐标系原点之间的距离；
11.3-2，姿态传感器获取自动引导车的姿态数据r(α,β,γ)，姿态传感器以数据处理器上电时为姿态零点r0(α0,β0,γ0)；其中，航向角α为以姿态零点r0(α0,β0,γ0)为起点，自动引导车在水平方向上移动的角度，顺时针移动的角度为正值，逆时针移动的角度为负值；β为俯仰角，γ为滚转角；
12.3-3，数据处理器利用位置数据p(a,z,d)、姿态数据r(α,β,γ)及上一时刻电机驱动器反馈的电机实际转动角度θ
feed
，计算出电机应执行的指令角度θ
cmd
；
13.3-4，电机驱动器将电机实际转动角度θ
feed
反馈给数据处理器，数据处理器计算电机应执行的输出角度θ
out
；数据处理器通过电机驱动器控制电机执行输出角度θ
out
，电机带动被测靶球的受光面主动对准激光跟踪仪的测试光源。
14.本发明将被测靶球的位置数据作为被测agv的位置数据，使安装在自动引导车上的被测靶球主动响应激光测量设备的测试光源，使激光跟踪仪能够实时测量被测的自动引导车的位置数据；
15.本发明采用无线操作，无需人工接触被测靶球即可完成agv性能测量；可动态测量，具备自动定位和跟踪活动目标的功能；测量方便，可以实时测出自动引导车在任何点位的位置数据和姿态数据。
16.计算机得到被测agv的位置数据、姿态数据后，可进行agv 的定位性能、重复定位性能、导航路程误差、导航时间误差等性能测试。
17.作为优选，步骤3-1包括如下具体步骤：
18.计算机利用位置数据p(a,z,d)计算坐标数据p
′
(px,py,pz)：其中，px＝d
×
sin(z)
×
sin(a)
19.py＝d
×
sin(z)
×
cos(a)
20.pz＝d
×
cos(z)；
21.计算机利用p
′
(px,py,pz)计算被测靶球的位置准确度和轨迹准确度，将被测靶球的位置准确度和轨迹准确度作为自动引导车的位置准确度和轨迹准确度。
22.作为优选，步骤3-3包括如下具体步骤：
23.3-3-1，数据处理器将位置数据p(a,z,d)中的被测靶球相对于激光跟踪仪的测量坐标系x轴的当前时刻偏移角度ai,和姿态数据r(α,β,γ)中的当前时刻航向角αi，均从[-π,π]转化到[0,2π]；
[0024]
3-3-2，数据处理器计算ai与上一时刻偏移角度a
i-1
的偏差dtai，αi与上一时刻航向角α
i-1
的偏差dtαi，将dtai和dtαi均用[0,2π]范围内的角度值表示；
[0025]
3-3-3，
[0026]
当dtai＜0、dtαi＜0并且dtai＞dtαi[0027]
或者dtai＞0、dtαi＞0并且dtai＞dtαi；
[0028]
表示被测靶球偏离激光跟踪仪的角度方向与其自转角度方向相同，则θ
cmd
＝dtai；
[0029]
当dtai和dtαi中一个为正值，另一个为负值，表示被测靶球偏离激光跟踪仪的角度方向与其自转角度方向相反，则θ
cmd
＝dtai+dtαi；
[0030]
当dtai和dtαi中的一个为零时，则θ
cmd
等于另一个不为零的角度值。
[0031]
作为优选，数据处理器利用θ
cmd
和θ
feed
计算出电机应执行的输出角度θ
out
：
[0032]
θ
out
＝k
p
(err-lasterr)+ki×
err
[0033]
err＝θ
cmd-θ
feed
；
[0034]
其中:k
p
为比例系数，ki为积分系数，err为当前时刻指令数据θ
cmd
与反馈数据θ
feed
之间的偏差值，lasterr为上一时刻指令数据θ
cmd
与反馈数据θ
feed
之间的偏差值。
[0035]
因此，本发明具有如下有益效果：被测靶球可主动响应激光测量设备的测试光源，
能够实时测量被测的自动引导车的位置数据、姿态数据；
[0036]
采用无线操作，无需人工接触被测靶球即可完成agv性能测量；具备自动定位和跟踪活动目标的功能，测量方便。
附图说明
[0037]
图1是本发明的一种结构示意图；
[0038]
图2是本发明的自动引导车、盒体、靶球座和被测靶球的一种结构示意图；
[0039]
图3是本发明的一种原理框图；
[0040]
图4是本发明的实施例的一种流程图。
具体实施方式
[0041]
下面结合附图和具体实施方式对本发明做进一步的描述。
[0042]
如图1、图2、图3所示的实施例是一种自动引导车的主动响应测量装置，包括计算机4，无线信号发射器41，激光跟踪仪42，设于自动引导车1上的盒体2，设于盒体中的数据处理器21、姿态传感器22、电机驱动器23、无线信号接收器24和电机25，设于盒体上方的靶球座3和设于靶球座上的被测靶球31；数据处理器分别与无线信号接收器、姿态传感器和电机驱动器数据连接，电机驱动器与电机数据连接，电机的转轴上端伸出盒体之外并与靶球座连接，计算机分别与无线信号发射器和激光跟踪仪数据连接，无线信号发射器和无线信号接收器无线连接，被测靶球位于激光跟踪仪的斜下方。
[0043]
本实施例中自动引导车为扫地机器人，姿态传感器为陀螺仪。
[0044]
如图4所示，一种自动引导车的主动响应测量装置的控制方法，包括如下步骤：
[0045]
3-1，激光跟踪仪测量被测靶球的位置数据p(a,z,d)；计算机控制无线信号发射器将位置数据p(a,z,d)中的a发送给无线信号接收器；
[0046]
计算机利用位置数据p(a,z,d)计算坐标数据p
′
(px,py,pz)：其中，px＝d
×
sin(z)
×
sin(a)
[0047]
py＝d
×
sin(z)
×
cos(a)
[0048]
pz＝d
×
cos(z)；
[0049]
计算机利用p
′
(px,py,pz)计算被测靶球的位置准确度和轨迹准确度，将被测靶球的位置准确度和轨迹准确度作为自动引导车的位置准确度和轨迹准确度。
[0050]
其中，a为在激光跟踪仪的测量坐标系下被测靶球相对于测量坐标系x轴的偏移角度；z为测量坐标系下被测靶球相对于测量坐标系 z轴正方向的夹角；d为被测靶球与测量坐标系原点之间的距离；
[0051]
3-2，姿态传感器获取自动引导车的姿态数据r(α,β,γ)，姿态传感器以数据处理器上电时为姿态零点r0(α0,β0,γ0)；其中，航向角α为以姿态零点r0(α0,β0,γ0)为起点，自动引导车在水平方向上移动的角度，顺时针移动的角度为正值，逆时针移动的角度为负值；β为俯仰角，γ为滚转角；
[0052]
3-3，数据处理器利用位置数据p(a,z,d)、姿态数据r(α,β,γ)及上一时刻电机驱动器反馈的电机实际转动角度θ
feed
，计算出电机应执行的指令角度θ
cmd
：
[0053]
3-3-1，数据处理器将位置数据p(a,z,d)中的被测靶球相对于激光跟踪仪的测量
坐标系x轴的当前时刻偏移角度ai,和姿态数据 r(α,β,γ)中的当前时刻航向角αi，均从[-π,π]转化到[0,2π]；
[0054]
3-3-2，数据处理器计算ai与上一时刻偏移角度a
i-1
的偏差dtai，αi与上一时刻航向角α
i-1
的偏差dtαi，将dtai和dtαi均用[0,2π]范围内的角度值表示；
[0055]
3-3-3，
[0056]
当dtai＜0、dtαi＜0并且dtai＞dtαi[0057]
或者dtai＞0、dtαi＞0并且dtai＞dtαi；
[0058]
表示被测靶球偏离激光跟踪仪的角度方向与其自转角度方向相同，则θ
cmd
＝dtai；
[0059]
当dtai和dtαi中一个为正值，另一个为负值，表示被测靶球偏离激光跟踪仪的角度方向与其自转角度方向相反，则θ
cmd
＝dtai+dtαi；
[0060]
当dtai和dtαi中的一个为零时，则θ
cmd
等于另一个不为零的角度值。
[0061]
3-4，电机驱动器将电机实际转动角度θ
feed
反馈给数据处理器，数据处理器计算电机应执行的输出角度θ
out
；数据处理器通过电机驱动器控制电机执行输出角度θ
out
，电机带动被测靶球的受光面主动对准激光跟踪仪的测试光源。
[0062]
其中，数据处理器利用θ
cmd
和θ
feed
计算出电机应执行的输出角度θ
out
：
[0063]
θ
out
＝k
p
(err-lasterr)+ki×
err
[0064]
err＝θ
cmd-θ
feed
；
[0065]
其中:k
p
为比例系数，ki为积分系数，err为当前时刻指令数据θ
cmd
与反馈数据θ
feed
之间的偏差值，lasterr为上一时刻指令数据θ
cmd
与反馈数据θ
feed
之间的偏差值。
[0066]
以上所述仅为本发明的较佳实施例，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。