一种机器人全自动运水的方法和运水机器人系统

摘要

一种机器人全自动运水的方法和运水机器人系统，系统包括机器人、机器人行走的道路和给水装置；机器人行走的道路中心埋置了电磁循迹线，能够产生交变磁场；机器人感应由电磁循迹线产生的交变磁场来识别路径。整个运水过程全自动化：机器人自动启动、机器人自动识别路径、水箱自动升降、机器人与给水装置间自动无线通信、给水装置自动给水、水箱自动储水、水位自动检测、机器人自动往返、机器人自动停止。机器人全自动运水的方法在机器人过弯道时通过计算机器人本体的前轮当前应偏转的角度β和速度v对机器人行走进行定量控制，控制精准，机器人不会偏离引导线，行进速度快。这种技术不受光线影响，不论光线如何变化都能正常循迹。

说明书

技术领域

本发明涉及智能机器人领域，具体涉及一种机器人全自动运水的方法。

本发明还涉及一种运水系统，尤其是无黑色引导线也可以循迹的运水机器人系统。

背景技术

目前，关于运水机器人的报道，只限于学生的课外制作或参加电子竞赛，它们通常都以MCS51单片机为控制核心，通常的数据传输方式是有线方式。如若通信双方通过无线进行通信，并控制执行机构执行动作，则通信双方通常都采用单片机来控制完成无线通信和动作执行。

传统的运水机器人循迹方法，通常采用红外光电循迹传感器，在黑色引导线引导下，进行红外光电循迹。这种方法，受光线影响很大，在光线强烈的地方，无法正常循迹。

传统的运水机器人在偏离轨迹时，采用机器人往左偏，轮子往右打；机器人往右偏，轮子往左打。控制方法是一种定性的控制方法。这种定性的技术，无法准确控制机器人在道路上行驶，机器人容易跑出道路，尤其当速度快时，更易冲出道路。

发明内容

本发明的目的是针对上述问题，提出一种机器人全自动运水的方法和运水机器人系统。

本发明的技术方案是：

一种机器人全自动运水的方法，机器人过弯道时通过计算机器人本体的前轮当前应偏转的角度β和速度v对机器人行走进行定量控制：

$>\beta =\arctan \frac{2\mathrm{d\Delta x}}{\Delta x^2+\Delta y^2+2\mathrm{d\Delta y}}$>

$>v=\frac{\left[\arcsin \right[(\frac{\mathrm{\Delta y}}{d}+1)\sin \beta ]-\beta \}d}{\Delta t\sin \beta }$>

其中：Δx为测得的机器人本体在Δt时间内的横向偏移量，Δy为测得的机器人本体在Δt时间内的纵向偏移量，d为已知的机器人本体的前车轴与后车轴的垂直距离。

机器人运水包括以下步骤：

(1)操作者通过无线遥控器发出指令，机器人系统接收指令并启动取水；

(2)机器人本体前部的电磁感应线圈传感器感应道路中间预埋电磁循迹线产生的交变磁场，通过电磁感应线圈传感器计算机器人本体在Δt时间内的横向偏移量Δx，基于Δx并通过测速传感器计算机器人本体在Δt时间内的纵向前进量Δy；

(3)机器人基于Δt、Δx、Δy，通过下式计算机器人本体的前轮当前应偏转的角度β和速度v：

$>\beta =\arctan \frac{2\mathrm{d\Delta x}}{\Delta x^2+\Delta y^2+2\mathrm{d\Delta y}}$>

$>v=\frac{\left[\arcsin \right[(\frac{\mathrm{\Delta y}}{d}+1)\sin \beta ]-\beta \}d}{\Delta t\sin \beta }$>

其中d为机器人本体的前车轴与后车轴的垂直距离；

(4)基于步骤3求得的β值，机器人进行判断：

当|β|≤6°时，判断机器人当前在直道上，舵机不打角，机器人不转弯；控制行走电机提速至直道最大速度；

当|β|＞6°时，判断机器人当前在弯道上或偏离电磁循迹线，控制舵机打β角，机器人转弯，当Δx<0，舵机往左打角，机器人往左转，当Δx>0，舵机往右打角，机器人往右转；控制行走电机降速至步骤(3)中求得的速度v值；

(5)机器人本体重复步骤(2)-(4)进行自动定量地识别路径，在道路上前进；

(6)位于机器人本体底部的干簧管与道路终点处的永久磁铁磁感应，机器人停止前进；

(7)机器人控制水箱上升至预设高度；

(8)机器人与给水装置通过红外通信模块建立连接，机器人发出开启注水的红外信号；

(9)给水装置接收开启注水的红外信号后，打开电磁水阀，将水注入机器人的水箱；

(10)水箱内的水位传感器检测到水位到达预设水位，发送结束注水的红外信号；

(11)给水装置接收结束注水的红外信号后，关闭电磁水阀，取水完毕；

(12)机器人控制水箱下降至原位；

(13)机器人本体重复步骤(2)-(4)进行自动定量地识别路径，在道路上返回；

(14)位于机器人本体底部的干簧管与道路起始处的永久磁铁磁感应，机器人停止前进；

(15)机器人本体控制安装在水箱中的抽水泵抽水至存水装置。

作为一种优选的横向偏移量Δx的计算方法，横向偏移量Δx是通过三个呈一横排排列、安装在机器人本体的前部并且横排方向与机器人前进方向垂直的电磁感应线圈传感器测量计算而得；其中，第一电磁感应线圈传感器设置在中间，第二、三电磁感应线圈传感器等距离分设在两端；测量方法为：沿着与宽度为w的机器人行走的道路相互垂直的方向，每隔Δw，采集第一电磁感应线圈传感器的感应电压AD1_i，从这些数据中，获得第一电磁感应线圈传感器的感应电压中的最大值max1、最小值min1，据此，求出：

第一电磁感应线圈传感器的感应电压输出相对值：

AD1_ir＝(AD1_i-min1)/(max1-min1)*100  (i＝1,2，…,w/Δw+1)

第一电磁感应线圈传感器的感应电压输出最大相对值：

AD1maxr＝(max1-min1)/(max1-min1)*100＝100

横向偏移量Δx的计算公式：

用AD1表示机器人前进过程中第一电磁感应线圈传感器测得的即时感应电压；用AD1_r表示第一电磁感应线圈传感器的即时感应电压输出相对值：

AD1_r＝(AD1-min1)/(max1-min1)*100

(I)Δx的大小：

|Δx|＝K0(AD1maxr-AD1_r)+K1

其中，常数K0和K1确定方法：根据以上采集到的w/Δw+1个感应电压AD1_i，以及每次采集数据时实际对应的Δx_i，使得达到最小值，即可以获得常数K0和K1的值(i＝1,2，…,w/Δw+1)；

(II)Δx的正负号：

(i)当左侧第二电磁感应线圈传感器的电感量大则：Δx＝-|Δx|；

(ii)当右侧第三电磁感应线圈传感器的电感量大则：Δx＝|Δx|。

作为一种优选的实施方式：w为40cm，Δw为0.5cm

更优的，机器人对速度检测并进行判断：

当目标速度减实际速度的差值超过预先设定的ΔVmax时，对行走电机输入正转最大占空比的PWM波；

当目标速度减实际速度的差值存在但未超过预先设定的ΔVmax时，对行走电机输入PWM控制信号：DrivingMotorOutput＝DrivingMotorOutput+(int)(p1*(error-pre1_error)+p2*(error)+p3*(error-2*pre1_error+pre2_error))，其中，p1，p2和p3是常数，error是本次速度检测时目标速度与实际速度的差值，pre1_error是上一次速度检测时目标速度与实际速度的差值，pre2_error是上两次速度检测时目标速度与实际速度的差值。

作为一种优选的实施方式：所述Δvmax＝38编码器脉冲个数/5ms，最大占空比＝58％。

一种机器人全自动运水方法的运水机器人系统，它包括机器人、给水装置和机器人行走的道路；

机器人包括机器人本体及布置其上的升降台、抽水泵、电机、电机驱动模块、舵机、水箱、水位传感器、第一MCU、干簧管、无线收发装置、电磁感应线圈传感器、测速传感器和第一红外通信模块，所述电机分为控制抽水泵的抽水泵电机、控制升降台的升降台电机、控制机器人行走的行走电机，三者均由第一MCU的驱动信号控制，其中行走电机有两个，分别安装在机器人本体的两侧，用于驱动机器人行进；所述的电磁感应线圈传感器安装在机器人本体的前部，感应由道路中心电磁循迹线产生的交变磁场，电磁感应线圈传感器的信号输出端与第一MCU的信号输入端相连；所述的舵机用于控制机器人转弯；所述的测速传感器安装在行走电机之上以检测机器人前进距离；所述的水箱安装在升降台的上方；所述抽水泵能够从水箱中抽水至外部储水装置；所述的水位传感器安装在水箱内，用于检测水位，水位传感器的检测信号输出端与第一MCU的检测信号输入端相连，第一MCU的控制信号输出端与第一红外通信模块的信号输入端相连；所述的第一红外通信模块安装在机器人本体的前部，用于与带有第二红外通信模块的给水装置进行通信；所述的干簧管安装到机器人底部，与安装在机器人行走的道路起点、终点处的永久磁铁磁感应连接，用于到位检测，所述的干簧管的检测信号输出端与第一MCU的控制信号输入端相连；所述的无线收发装置与操作者手持的遥控器进行无线连接，该无线收发装置与第一MCU双向连接，当操作者按动遥控器的启动按钮，启动机器人系统自动运水；

给水装置包括给水桶、安装在给水桶外壁上的第二红外通信模块，安装在给水桶底部的电磁水阀和第二MCU，所述的第二MCU的控制信号输出端与电磁水阀的控制信号输入端相连，第二MCU与第二红外通信模块相连，第二红外通信模块与机器人的第一红外通信模块无线连接，进行放水和停止放水的操作；

机器人行走的道路中心埋置了电磁循迹线，能够产生交变磁场；在该道路的终点处和起始处，安装了永久磁铁，用于与机器人底部的干簧管磁感应连接。

作为一种优选的实施方案，电磁水阀为零压差起动电磁水阀。

所述的升降台包括上台板、下底板、剪叉架、旋转轴、升压模块、电机驱动模块和升降台电机，所述的升压模块的信号输入端与电源连接，升压模块的输出端通过电机驱动模块为升降台电机供电，所述的升降台电机的输出轴连接旋转轴的一端，旋转轴的另一端连接剪叉架，下底板安装在剪叉架的底部，上台板安装在剪叉架的顶部，水箱安装在上台板上。

更具体的，所述抽水泵电机、升降台电机、行走电机的控制信号输入端经相对应的驱动模块，与第一MCU相对应的控制信号输出端相连，抽水泵电机、升降台电机、行走电机均通过电机驱动模块被第一MCU驱动，其中抽水泵电机的驱动模块是三极管驱动电路，升降台电机和行走电机的驱动模块为H桥驱动电路。第一MCU的输出信号通过H桥驱动行走电机旋转，实现机器人本体的行走；通过H桥驱动升降台电机旋转，实现升降台的升降；通过三极管驱动抽水泵电机旋转，实现抽水；舵机的控制信号输入端，直接与第一MCU相对应的控制信号输出端相连，第一MCU直接驱动舵机打角，实现机器人本体的转弯。

作为一种优选的实施方案，所述的电磁感应线圈传感器为三个，呈一横排安装在机器人本体的前部，与机器人前进方向垂直。

更优的，所述的机器人本体还包括TFT-LCD显示屏，第一MCU的显示信号输出端与TFT-LCD显示屏的信号输入端相连。

本发明的有益效果

本发明的机器人全自动运水的方法，整个运水过程全自动化：机器人自动启动、机器人自动识别路径、水箱自动升降、机器人与给水装置间自动无线通信、给水装置自动给水、水箱自动储水、水位自动检测、机器人自动往返、机器人自动停止。

本发明的机器人全自动运水的方法，提供了一种在机器人过弯时的全新的行驶控制算法：通过环境参数来计算机器人当前应打过的角度β和应该给出的速度v，实现了当机器人偏离引导线时的定量控制。定量控制中，角度β能够保证机器人在道路上准确行驶；速度v能够保证机器人平稳地通过弯道。这种定量控制相比于定性控制，极大的提高了机器人在行驶过程中的稳定性，运水效率也大为提高。

本发明的机器人全自动运水的方法，在求得角度β值后，根据β值的大小选取控制策略，在保证机器人不脱离导轨前提下的高速行走。在目标速度与实际速度存在差值时，采取脉宽调制方法进行精确控制。

本发明的运水机器人系统，通过红外通信方式巧妙地解决了机器人、给水装置之间的通信问题，而且是无线通信，不需要连线，使得操作更加便利。

传统运水机器人系统所使用的电磁水阀，不是零压差起动，其两端需要有一定的压差，水阀才能打开，若没有水压差，即使电磁水阀通电，水阀也无法打开。而应用在我们这里会出现因给水装置的水压通常达不到电磁水阀打开的最小水压差，所以是无法使用的问题。针对此问题，本发明的方案采用零压差起动的电磁水阀，无需要压差，只要电磁水阀通电，即可打开它。

本发明的运水机器人系统，采用电磁感应线圈传感器，感应由道路中心电磁循迹线产生的交变磁场，来识别路径。这种技术不受光线影响，不论光线如何变化(强或弱)，都能正常循迹，且适于在恶劣环境下，道路表面很脏乱，甚至泥泞的道路表面，机器人都能够正常运水，受道路环境影响小。

本发明的运水机器人系统，操作者通过无线方式，控制机器人自动取水，代替了传统的通过按下启动按钮的方法，启动机器人取水。提高了操作的便捷性、舒适性，更加具有人性化。

附图说明

图1为本发明运水机器人系统的结构框图。

图2为本发明运水机器人系统的升降台的结构框图。

图3为本发明机器人全自动运水方法的横向偏移量Δx计算的原理图。

图4为本发明机器人全自动运水方法的过弯角度与速度计算的原理图。

具体实施方式

下面结合实施例对本发明作进一步说明，但本发明的保护范围不限于此：

下面结合附图和实施例对本发明作进一步的说明。

如图1所示，本发明运水机器人系统，它包括机器人、机器人行走的道路、给水装置；

机器人包括机器人本体及布置其上的升降台、抽水泵、电机、电机驱动模块、舵机(型号可为FUTABA3010)、水箱、水位传感器、第一MCU(型号可为STM32F407ZE)、干簧管、无线收发装置、电磁感应线圈传感器、测速传感器和第一红外通信模块，所述电机分为控制抽水泵的抽水泵电机(型号可为自吸式微型潜水泵)、控制升降台的升降台电机(型号可为减速电机GM48-520)、控制机器人行走的行走电机(型号可为280电机)，三者均由第一MCU的驱动信号控制，其中行走电机有两个分别安装在机器人本体的两侧；所述的电磁感应线圈传感器安装在机器人本体的前部，感应由道路中心电磁循迹线产生的交变磁场，电磁感应线圈传感器的信号输出端与第一MCU的信号输入端相连；所述的舵机用于控制机器人转弯；所述的测速传感器安装在行走电机之上以检测机器人前进距离：根据单位时间测速传感器记得的脉冲数，算出机器人前进的距离；所述的水箱安装在升降台的上方；所述抽水泵能够从水箱中抽水至外部储水装置；所述水位传感器安装在水箱内，用于检测水位，水位传感器的检测信号输出端与第一MCU的检测信号输入端相连，第一MCU的控制信号输出端与第一红外通信模块的信号输入端相连；所述的第一红外通信模块安装在机器人本体的前部，用于与带有第二红外通信模块的给水装置进行通信；所述的干簧管安装到机器人底部，与安装在机器人行走的道路起点、终点处的永久磁铁磁感应连接，用于到位检测，所述的干簧管的检测信号输出端与第一MCU的控制信号输入端相连；所述的无线收发装置与操作者手持的遥控器进行无线连接，该无线收发装置与第一MCU双向连接，当操作者按动遥控器的启动按钮，启动机器人系统自动运水；

给水装置包括给水桶、安装在给水桶外壁上的第二红外通信模块，安装在给水桶底部的电磁水阀(零压差起动电磁水阀)和第二MCU(型号可为STM32F103CB)，所述的第二MCU的控制信号输出端与电磁水阀的控制信号输入端相连，第二MCU与第二红外通信模块相连，第二红外通信模块与机器人的第一红外通信模块无线连接，进行放水和停止放水的操作；

机器人行走的道路中心埋置了电磁循迹线，能够产生交变磁场；在该道路的终点处和起始处，安装了永久磁铁，用于与机器人底部的干簧管磁感应连接。

如图2所示，作为升降台的一个优选实施方式：升降台包括上台板、下底板、剪叉架、旋转轴、升压模块、电机驱动模块和升降台电机，所述的升压模块的信号输入端与电源连接，升压模块的输出端通过电机驱动模块为升降台电机供电，所述的升降台电机的输出轴连接旋转轴的一端，旋转轴的另一端连接剪叉架，下底板安装在剪叉架的底部，上台板安装在剪叉架的顶部，水箱安装在上台板上。

作为电机的一个优选驱动方式：抽水泵电机、升降台电机、行走电机的控制信号输入端经相对应的驱动模块，与第一MCU相对应的控制信号输出端相连，抽水泵电机、升降台电机、行走电机均通过电机驱动模块被第一MCU驱动，其中抽水泵电机的驱动模块是三极管驱动电路，升降台电机和行走电机的驱动模块为H桥驱动电路；第一MCU的输出信号通过H桥驱动行走电机旋转，实现机器人本体的行走；通过H桥驱动升降台电机旋转，实现升降台的升降；通过三极管驱动抽水泵电机旋转，实现抽水；舵机的控制信号输入端，直接与第一MCU相对应的控制信号输出端相连，第一MCU直接驱动舵机打角，实现机器人本体的转弯。

作为一个优选实施方式，电磁感应线圈传感器为三个且呈一横排排列安装在机器人本体的前部，横排方向与机器人前进方向垂直。

在一个优选的实施方式中，机器人本体还包括TFT-LCD显示屏，第一MCU的显示信号输出端与TFT-LCD显示屏的信号输入端相连。

本发明的机器人全自动运水的方法如技术方案所述，这里不再进行赘述。下面对本发明机器人全自动运水方法的横向偏移量Δx、纵向前进量Δy以及机器人过弯时机器人本体的车轮当前应偏转的角度β和速度v的求解原理进行详细说明。

以下结合图3对本发明机器人全自动运水方法的横向偏移量Δx的计算进行说明：

如图3所示，横向偏移量Δx是通过三个呈一横排排列、安装在机器人本体的前部并且横排方向与机器人前进方向垂直的电磁感应线圈传感器测量计算而得；其中，第一电磁感应线圈传感器设置在中间，第二、三电磁感应线圈传感器等距离分设在两端；测量方法为：沿着与宽度为40cm的机器人行走的道路相互垂直的方向，每隔0.5cm，采集第一电磁感应线圈传感器的感应电压AD1_i，从这些数据中，获得第一电磁感应线圈传感器的感应电压中的最大值max1、最小值min1，据此，求出：

第一电磁感应线圈传感器的感应电压输出相对值：

AD1_ir＝(AD1_i-min1)/(max1-min1)*100  (i＝1,2，…,81)

第一电磁感应线圈传感器的感应电压输出最大相对值：

AD1maxr＝(max1-min1)/(max1-min1)*100＝100

横向偏移量Δx的计算公式：

用AD1表示机器人前进过程中第一电磁感应线圈传感器测得的即时感应电压；用AD1_r表示第一电磁感应线圈传感器的即时感应电压输出相对值：

AD1_r＝(AD1-min1)/(max1-min1)*100

(I)Δx的大小：

|Δx|＝K0(AD1maxr-AD1_r)+K1

其中，常数K0和K1确定方法：根据以上采集到的81个感应电压AD1_i，以及每次采集数据时实际对应的Δx_i，使得达到最小值，即可以获得常数K0和K1的值(i＝1,2，…,81)；

(II)Δx的正负号：

(i)当左侧第二电磁感应线圈传感器的电感量大则：Δx＝-|Δx|；

(ii)当右侧第三电磁感应线圈传感器的电感量大则：Δx＝|Δx|。

以下对本发明机器人全自动运水方法的纵向前进量Δy的计算进行说明：

在车轮上装上测速传感器，根据Δt时间内测速传感器记得的脉冲数，可算出小车Δt时间前进的距离Δs。若小车一直在直道上行驶，没有偏离电磁循迹线，则这个距离就是Δy，若小车偏离电磁循迹线，则

以下结合附图4对机器人过弯时机器人本体的车轮当前应偏转的角度β和速度v的求解原理进行分析：

如图4所示，机器人本体的前车轴作为x轴，前车轴的中点作为原点A，在原点处的x轴垂线作为y轴，建立坐标系。其中机器人前进方向为y轴正方向，机器人右方为x轴正方向，电磁感应线圈传感器轴(其上放置了3个电磁感应线圈传感器)架到距离机器人前车轴为L的位置，其与y轴的交点记为B点，机器人前车轴到后车轴的距离为d。

由B点向电磁循迹线，做切线，机器人沿该切线方向前进，机器人的前轮需要偏转β角度、速度为v。此时，点A围绕某圆心做圆周运动。根据图示中的几何关系，得出该运动圆心为(d/tanβ,-d)，半径R为d/sinβ，该圆的一般方程为：

$>{(x-\frac{d}{\tan \beta })}^2+{(y+d)}^2={\left(\frac{d}{\sin \beta }\right)}^2$>

其参数方程为：

$>\left(\begin{array}{c}x=\frac{d}{\sin \beta }(\frac{d}{\tan \beta }+\cos \theta )\\ y=\frac{d}{\sin \beta }\sin \theta \end{array}\right)$>

进一步分析可知图4中包含以下关系式：

>

其中：Δβ是机器人的前轮在Δt时间内偏转角度β的变化量。ω是点A处的角速度，v是点A处的线速度，R是点A处的拐弯半径，等于d/sinβ。

可知下式成立：

$>\mathrm{\Delta \beta }=\frac{\mathrm{v\Delta }t\sin \beta }{d}---\left(1\right)$>

Δx、Δy、Δβ满足：

$>\left(\begin{array}{c}\mathrm{\Delta x}=R\cos \beta -R\cos (\beta +\mathrm{\Delta \beta })\\ \mathrm{\Delta y}=R\sin (\beta +\mathrm{\Delta \beta })-R\sin \beta \end{array}\right)---\left(2\right)$>

结合R＝d/sinβ，有：

$>\left(\begin{array}{c}\mathrm{\Delta x}=\frac{d}{\sin \beta }[\cos \beta -\cos (\beta +\mathrm{\Delta \beta })]\\ \mathrm{\Delta y}=\frac{d}{\sin \beta }[\sin (\beta +\mathrm{\Delta \beta })-\sin \beta ]\end{array}\right)$>

结合式(1)，有：

$>\left(\begin{array}{c}\mathrm{\Delta x}=\frac{d}{\sin \beta }[\cos \beta -\cos (\beta +\frac{\mathrm{v\Delta }t\sin \beta }{d})]\\ \mathrm{\Delta y}=\frac{d}{\sin \beta }[\sin (\beta +\frac{\mathrm{v\Delta }t\sin \beta }{d})-\sin \beta ]\end{array}\right)$>

等式两边同时除以

$>\left(\begin{array}{c}\mathrm{\Delta x}\frac{\sin \beta }{d}=\cos \beta -\cos (\beta +\frac{\mathrm{v\Delta }t\sin \beta }{d})\\ \mathrm{\Delta y}\frac{\sin \beta }{d}=\sin (\beta +\frac{\mathrm{v\Delta }t\sin \beta }{d})-\sin \beta \end{array}\right)$>

方程式移项：

$>\left(\begin{array}{c}\cos (\beta +\frac{\mathrm{v\Delta }t\sin \beta }{d})=\cos \beta -\mathrm{\Delta x}\frac{\sin \beta }{d}\\ \sin (\beta +\frac{\mathrm{v\Delta }t\sin \beta }{d})=\sin \beta +\mathrm{\Delta y}\frac{\sin \beta }{d}\end{array}\right)---\left(3\right)$>

可得：

$>{(\cos \beta -\mathrm{\Delta x}\frac{\sin \beta }{d})}^2+{(\sin \beta +\mathrm{\Delta y}\frac{\sin \beta }{d})}^2=\cos {(\beta +\frac{\mathrm{v\Delta }t\sin \beta }{d})}^2+\sin {(\beta +\frac{\mathrm{v\Delta }t\sin \beta }{d})}^2=1$>

展开：

$>{\left(\mathrm{\Delta x}\frac{\sin \beta }{d}\right)}^2-2\cos \mathrm{\beta \Delta x}\frac{\sin \beta }{d}+{\left(\mathrm{\Delta y}\frac{\sin \beta }{d}\right)}^2+2\sin \mathrm{\beta \Delta y}\frac{\sin \beta }{d}=0$>

等式两边约去sinβ：

$>\Delta x^2\frac{\sin \beta }{d^2}-2\mathrm{\Delta x}\frac{\cos \beta }{d}+\Delta y^2\frac{\sin \beta }{d^2}+2\mathrm{\Delta y}\frac{\sin \beta }{d}=0$>

移项：

$>(\frac{\Delta x^2}{d^2}+\frac{\Delta y^2}{d^2}+\frac{2\mathrm{\Delta y}}{d})\sin \beta =2\mathrm{\Delta x}\frac{\cos \beta }{d}$>

等式两边同时乘以d²:

(Δx²+Δy²+2Δy d)sinβ＝2Δx d cosβ

等式两边同时除以cosβ：

(Δx²+Δy²+2Δy d)tanβ＝2Δx d

即：

$>\tan \beta =\frac{2\mathrm{\Delta xd}}{\Delta x^2+\Delta y^2+2\mathrm{\Delta yd}}$>

可获得角度β和速度v的表达式为：

$>\beta =\arctan \frac{2\mathrm{d\Delta x}}{\Delta x^2+\Delta y^2+2\mathrm{d\Delta y}}---\left(4\right)$>

同理，将式(4)代入式(3)的第二个方程，推导可获得：

$>v=\frac{\left[\arcsin \right[(\frac{\mathrm{\Delta y}}{d}+1)\sin \beta ]-\beta \}d}{\Delta t\sin \beta }$>

本文中所描述的具体实施例仅仅是对本发明精神做举例说明。本发明所属技术领域的技术人员可以对所描述的具体实施例做各种各样的修改或补充或采用类似的方式替代，但并不会偏离本发明的精神或者超越所附权利要求书所定义的范围。

本发明未涉及部分均与现有技术相同或可采用现有技术加以实现。