CN106919185B - 基于Dubins曲线的稳定平台位置伺服控制方法 - Google Patents

Abstract

本发明公开了一种基于Dubins曲线的稳定平台位置伺服控制方法。该方法基于Dubins曲线，将当前位置状态和需达到的位置状态通过该曲线连接，并将该曲线作为位置曲线指令传递给执行机构位置环路。该曲线具备以下优点：可使执行机构从当前位置状态启动，稳定光滑地过渡到以某一固定速度行进，最后回到静止状态；可使执行机构从当前位置状态，快速无超调地到达另一位置状态，达到该位置后立刻保持静止。本发明解决了现有技术在位置曲线指令规划时速度转换的不平滑问题，提高了执行机构在执行位置指令时速度转换的稳定性。同时本发明缩短了执行机构的响应时间，进一步抑制了执行机构在执行位置指令时所产生的超调。本发明对于提高稳定平台执行机构的快速响应性和稳定性都有重要的意义。

Description

基于Dubins曲线的稳定平台位置伺服控制方法

技术领域

本发明属于伺服控制领域，涉及一种位置伺服控制方法，尤其涉及一种基于Dubins曲线的航空相机位置指令伺服控制方法。

背景技术

航空相机系统是在高空中利用高分辨率传感器，实时获取地面一定区域的图像或视频信息的成像系统。航空相机系统除了需要高分辨率传感器之外，还需要能满足其稳定和调转的伺服执行机构。

航空相机系统的传感器既需要在保持惯性稳定在某一位置下静态拍摄获取图像，又需要在以某一恒定速度运动下动态拍摄获取图像。因此在静态拍照时保持高精度惯性稳定，在动态拍照时候保持速度平稳且能抵抗干扰，在动静切换时保证位置指向精确快速无超调，这是系统对伺服执行机构的要求。

设计航空相机的伺服稳定平台，可以抵抗外界的干扰，保证相机系统的惯性稳定。其稳定平台包括三级控制回路，电流反馈回路，速度控制回路，位置控制回路。位置调转是稳定平台伺服机构经常使用的功能。伺服机构的位置调转响应的快速性和到位的精准性，与伺服系统各个环路的时频域参数息息相关。当伺服系统的环路内的固有属性和校正控制器确定后，系统的快速调转的响应性能就已经确定。若无位置曲线指令规划，直接对系统进行位置调转和速度切换，系统将会产生超调震荡，不利于系统快速达到稳定的拍照状态，甚至可能导致伺服系统的不稳定。

根据Dubins曲线理论，在二维平面中两个向量之间最短的路径是圆弧及其切线组成，路径的形状包括CLC,CCL等，C表示圆弧，L表示直线段，圆弧是二维平面内矢量旋转的曲线中阶数最少的，而直线段是两点间距离最短的。基于此，本发明将位置时间坐标系中的位置指令，通过圆弧及其相切的直线段来连接，通过这种方式生成位置指令，可满足伺服机构位置快速无超调调转，或者以要求的速度行进。

本发明的控制方法根据当前时刻的位置值和速度值，以及目标时刻的位置值和速度值，来计算出一条位置曲线。这个位置曲线指令光滑连接系统需要达到的一些位置状态。通过计算出的这个曲线，生成了位置指令，并给到位置环路输入中，通过环路生成驱动命令，给到执行机构上来最后到达所期望的运动状态。

中国专利201310676886.7公开了一种基于Dubins路径和稀疏A*搜索的无人机航迹规划方法，在二维飞行路线中，将当前飞机状态，通过一种路径规划方法，最终把路径连接到飞机需要到达的最终状态。属于航迹自主规划技术领域，与本发明所采用的原理类似。中国专利201410526384.0公开了一种航空相机稳定平台无超调伺服控制方法，属于自动控制技术领域。该专利对横滚通道位置指令进行规划，得到一条从初始姿态角度按照1/4正弦曲线过渡到位置指令角度的曲线。该与本发明属于同一领域，都是对位置曲线的规划。但该发明仅考虑到位置指令规划，并无对速度指令的规划。在本发明项目背景下，传感器需要在固定的速度行进中工作。这就要求系统不仅能从当前状态开始到一定角度停止后再进行拍照，而且还要从当前状态到以另一固定速度行进中拍照。这两种过程都需要平稳快速无超调，本发明基于此进行设计。

发明内容

要解决的技术问题

本发明要解决的技术问题是：设计一种伺服位置曲线使伺服执行机构快速无超调地响应静止-加速-匀速-减速-静止等状态之间的切换，并且状态保持或者切换时过程快速且平稳，同时在静止状态下要保证惯性稳定。

据此问题，本发明提出一种基于Dubins曲线的位置指令伺服控制方法，本发明通过此种设计，生成了一种快速响应平滑过渡的位置曲线，并且使伺服执行机构按照设定的位置曲线来执行位置指令。并应用于一种航空相机平台上。

该航空相机稳定平台俯仰方向保持与地面垂直，利用本发明位置控制方法对其横滚方向进行位置指令规划。

本发明的技术方案为：

所述一种基于Dubins曲线的稳定平台位置伺服控制方法，其特征在于：包括以下步骤：

步骤1：将起始位置横滚和俯仰瞄准线初始角度值分别对应赋值给横滚和俯仰位置环输入，使伺服机构位置调转，初始化到起始位置；

步骤2：将相机系统垂直于地面的俯仰角度值赋给目标俯仰角，使相机俯仰方向垂直于地面，并保持惯性稳定；

步骤3：获取相机系统横滚方向的工作任务指令，得到当前横滚方向的工作状态；根据所获取的当前横滚方向工作状态，将当前工作任务指令发出给执行机构；执行机构判断是否收到工作任务指令，如果没有收到时，继续等待接收；如果收到指令，则进入步骤4；

步骤4：判断当前工作状态是按照固定速度动态过程拍照，还是到达固定位置静止进行拍摄；如果是按照固定速度动态过程拍照，则进入步骤5，如果是到达固定位置静止进行拍摄，则进入步骤11；

步骤5；执行机构接收行进拍照的速度值g_m_photo_vel和拍摄区间[g_m_photo_start，g_m_photo_stop]；判断接收的速度值是否处于执行机构自身的可实现速度范围内，如果不是，则重新接收速度值，如果是，则进入步骤6；

步骤6：生成动态拍照位置指令曲线，所述动态拍照位置指令曲线为位置-时间曲线，由圆弧段L1、直线段L2和减速段L3组成；圆弧段L1初始点为(T0,Y0)，匀直线段L2起始点为(T1,Y1)，直线段L2的速度为K，圆弧段L1的半径为R，圆弧的圆心坐标(Tr,Yr)，T为当前时刻；

当瞄准线角度值逐渐增大时，圆弧段L1的曲线公式为当瞄准线角度值逐渐减小时，圆弧段L1的曲线公式为

直线段L2的公式为Y＝K*T+(Y1-K*T1)；

减速段L3起点为(T2,Y2)，减速段圆弧半径为R，减速段圆弧圆心坐标(Tr’,Yr’),减速段终点坐标为(T3,Y3)

当瞄准线角度值逐渐增大时，减速段L3的曲线公式为当瞄准线角度值逐渐减小时，减速段L3的曲线公式为

步骤7：根据当前时刻T解算位置指令曲线，并将解算得到的与时间T相关的位置指令Y实时传给横滚位置环路的位置命令g_comd_ro_pos；根据横滚和俯仰旋变实时解算当前瞄准线横滚角g_los_angles.ro；将位置命令g_comd_ro_pos与当前瞄准线横滚角g_los_angles.ro做差得到位置偏差dbposErr_io＝g_comd_ro_pos-g_los_angles.ro；对位置偏差经过PI位置控制器校正后得到校正位置偏差out_pi_ro；将校正位置偏差out_pi_ro经过超前控制器校正得到超前控制器横滚输出结果out_lead_ro，将超前控制器横滚输出结果赋值给速度环命令g_ia_cmnd＝out_lead_ro；将速度环命令进行限幅并输出到横滚电机；

步骤8：判断当前瞄准线角度是否大于拍照起始角度g_m_photo_start，如果是，则进入步骤9，否则返回步骤7；

步骤9：执行机构进入动态拍照阶段，给相机发送运动状态连续拍照指令，在匀速行进中进行拍摄；判断当前瞄准线角度是否大于拍照结束角度g_m_photo_stop，如果到达则动态拍照段终止，给相机发送停止拍照指令，否则返回步骤7；

步骤10：判断是否收到工作结束指令，如果是则结束工作流程；否则返回步骤3；

步骤11：设置航道间隔固定角度，相机每经过固定间隔的角度后开始拍照；判断设置航道间隔固定角度是否符合执行机构可实现的角度范围，如果是，则进入步骤12，否则重新设置航道间隔固定角度；

步骤12：生成静态拍照位置指令曲线，所述静态拍照位置指令曲线为位置-时间曲线，由圆弧段L1、直线段L2和减速段L3组成；圆弧段L1初始点为(T0,Y0)，匀直线段L2起始点为(T1,Y1)，直线段L2的速度为K，K为执行机构所能跟上位置指令的最大速度，ΔT为选取的T1和T0的间隔时间，AC为执行机构所能跟上的位置指令的最大加速度，圆弧段L1的半径为R，圆弧的圆心坐标(Tr,Yr)，T为当前时刻；

当瞄准线角度值逐渐增大时，圆弧段L1的曲线公式为当瞄准线角度值逐渐减小时，圆弧段L1的曲线公式为

直线段L2的公式为Y＝K*T+(Y1-K*T1)；

减速段L3起点为(T2,Y2)，减速段圆弧半径为R，减速段圆弧圆心坐标(Tr’,Yr’),减速段终点坐标为(T3,Y3)

当瞄准线角度值逐渐增大时，减速段L3的曲线公式为当瞄准线角度值逐渐减小时，减速段L3的曲线公式为

步骤13：根据当前时刻T解算位置指令曲线，并将解算得到的与时间T相关的位置指令Y实时传递给位置环；位置环输入指令经过数字位置控制器校正，传递到速度环，然后经过数字速度控制器校正后，生成速度命令输出到横滚电机；

步骤14：判断是否达到目标瞄准线角度位置，如果是，则给相机发单次拍照指令，同时开始对拍照计时；否则返回步骤13；判断单次拍照时间是否到，如果时间到，则给照相机发射停止拍照指令；

步骤15：判断是否收到工作结束指令，如果是则结束工作流程，否则，返回步骤3。

有益效果

本发明有以下有益效果：

1、本发明按照实际需要的运动速度设计位置曲线，在动静切换时速度曲线也符合稳定平台实际要求，现有位置曲线规划技术并未基于此进行设计。

2、利用本发明设计位置指令曲线，执行机构可快速无超调响应。与现有技术相比，在同样时间区间的位置曲线指令的下，执行机构响应本发明位置曲线的结果更平滑超调更小，执行机构到位需要的时间更短。

3、易于软件实现，可在软件中嵌入该方法实时生成曲线。

4、适用范围广，伺服执行机构位置指令设计均可采用。

附图说明

图1是本发明实施例的伺服控制方法的工作流程图。

图2是实施例静态拍照位置指令曲线图，其中实线为位置曲线指令，虚线为位置曲线圆弧段所在的圆。

图3是实施例动态拍照位置指令曲线图，其中实线为位置曲线指令，虚线为位置曲线圆弧段所在的圆。

图4a是用现有技术(正弦)进行静态拍照位置曲线规划时系统的跟踪响应。其中左边的曲线为静态拍照位置指令曲线，右边的曲线为系统响应指令的实际曲线。

图4b是用本发明进行静态拍照位置曲线规划时系统的跟踪响应。其中左边的曲线为静态拍照位置指令曲线，右边的曲线为系统响应指令的实际曲线。

图5是用本发明进行动态拍照位置曲线规划时系统的跟踪响应。其中左边的曲线为动态拍照位置指令曲线，右边的曲线为系统响应指令的实际曲线。

具体实施方式

下面结合附图及优选实施例对本发明作进一步的详述。

本发明的实施例是一个高分辨率航空数码相机。航空相机稳定平台包括外横滚框架和内俯仰框架。外横滚框架安装在载机上，内俯仰安装在外横滚框架内。双轴速率陀螺安装在内俯仰框架中，其两个轴分别和横滚和俯仰两个旋转轴平行，分别敏感横滚和俯仰方向的角速度。横滚和俯仰变压器用来测定横滚和俯仰方向的角位置信息。横滚旋转变压器装在横滚轴上，俯仰变压器安装在俯仰轴上。相机固定安装在内俯仰框架中。相机通过RS232接口和伺服计算机(DSP)通讯，伺服计算机给相机发送拍照指令。该实施例要求稳定平台动态拍照和静态拍照周期交替执行。

动态拍照时，稳定平台从静止状态加速到12度/秒的速度，到达拍照起始角度时立刻发送连续拍照指令给相机，并保持当前速度运行。拍照区间是瞄准线在1度到12度。到达12度后给相机发出停止拍照指令。相机停止拍照后，执行机构逐渐减速停止。执行机构停止后，迅速反向启动，以最快速度回到初始零位位置，该过程执行周期为1.8秒。

静态拍照时，稳定平台每间隔3度进行一次单次拍照，从初始零位开始，每到达一个新的角度间隔位置后保持静止状态，并给相机发送单次拍照指令，等待一定的拍照时间后再次启动到达下一个拍照航道。在瞄准线角度到达18度时，反向启动，以最快速度回到第一个航道进行拍摄，该过程执行周期为2.0秒。

本发明通过嵌入式软件代码实现，运行于伺服计算机中。当伺服计算机上电后，伺服软件模块将按照图1所示的工作流程执行以下步骤。

步骤1：伺服控制系统启动后，将伺服计算机硬件寄存器和软件变量初始化：将起始位置横滚和俯仰瞄准线初始角度值θ_init_ro和θ_init_el分别对应赋值给横滚和俯仰位置环输入g_comd_ro_pos和g_comd_el_pos，使伺服机构位置调转，初始化到起始位置；

g_comd_ro_pos＝θ_init_ro，

g_comd_el_pos＝θ_init_el。

步骤2：将相机系统垂直于地面的俯仰角度值赋给目标俯仰角，

g_comd_el_pos＝θ_angle_el

使相机俯仰方向垂直于地面，并保持惯性稳定。

步骤3：根据当前工作周期，获取相机系统横滚方向的工作任务指令，工作任务指令可分为动态拍照指令g_photo_move_mode和静态拍照指令g_photo_stable_mode，得到当前横滚方向的工作状态。根据所获取的当前横滚方向工作状态，将当前工作任务指令发出给执行机构；执行机构判断是否收到工作任务指令，如果没有收到时，继续等待接收；如果收到指令，则进入步骤4；

步骤4：判断当前工作状态是按照固定速度动态过程拍照，还是到达固定位置静止进行拍摄；如果是按照固定速度动态过程拍照，则进入步骤5，如果是到达固定位置静止进行拍摄，则进入步骤11；

步骤5；执行机构接收行进拍照的速度值g_m_photo_vel和拍摄区间[g_m_photo_start，g_m_photo_stop]。

拍摄区间为：拍照起始角度g_m_photo_start＝1，拍照终止角度g_m_photo_stop＝12。

由于执行机构本身的加速度和速度限制，判断接收的速度值是否处于执行机构自身的可实现速度范围内，如果不是，则重新接收速度值，如果是，则进入步骤6。实施例中设定的速度可取范围在g_m_photo_vel>0度/秒且g_m_photo_vel<60度/秒。

步骤6：生成动态拍照位置指令曲线；

根据Dubins曲线理论，采用CLC曲线来实现，将位置指令曲线分为3段。第一段L1为圆弧段，用于从静止加速到目标速度。第二段L2为直线段，该段保持匀速运动。第三段L3为减速段，该段使执行机构从某一速度逐渐减速至静止。

L1段：根据匀速段的速度设定，设计曲线圆弧L1段使执行机构由静止加速至目标速度。在位置-时间曲线中，初始时刻为(T0,Y0)其中T0＝0,Y0＝0。匀速阶段起始时间为T1＝0.1，匀速段设定速度K为12度/秒，加速段圆弧半径为R。圆弧的圆心坐标(Tr,Yr)，T为当前时刻。利用以下公式进行求解：

当瞄准线角度值逐渐增大时，圆弧段L1的曲线公式为当瞄准线角度值逐渐减小时，圆弧段L1的曲线公式为

本实施例中，本段瞄准线角度值逐渐增大：

Tr＝T0，θ＝arctanK，其中θ为圆弧段对应的圆心角度，圆弧半径为：Y1＝Y0+R-Rcosθ，圆心纵坐标：Yr＝Y0+R；

加速段曲线生成公式为：T为当前时刻。

L2段：匀速段起始时刻为T1，终止时刻为T2＝1.1，

匀速段曲线生成公式为：Y＝K*T+(Y1-K*T1)，其中T1≤T≤T2。

L3段：设计L3段为减速段，本段使执行机构从匀速运动状态转为静止状态。减速阶段起点为(T2,Y2)，减速段圆弧半径为R，圆弧的圆心坐标(Tr’,Yr’)，减速段终点坐标为(T3,Y3)，T3＝1.2其中R，(Tr’,Yr’)为待求解值，T为当前时刻。利用以下公式进行求解：

当瞄准线角度值逐渐增大时，减速段L3的曲线公式为当瞄准线角度值逐渐减小时，减速段L3的曲线公式为

本实施例中本段瞄准线角度值逐渐增大：

Tr′＝T3，θ＝arctanK，其中θ为圆弧段对应的圆心角度，圆弧半径为：Y3＝Y2+R-R*cosθ，Yr′＝Y3-R；

减速段曲线生成公式为：

L4段：本段为静止段，执行机构保持在Y3角度不动，T4＝1.3,Y4＝Y3。

L5段：T5＝1.4；本段瞄准线角度值逐渐减小：

Tr＝T4，θ＝arctanK1，其中K1为系统返回时的速度，Y5＝Y4-(R-R*cosθ)，其中θ为圆弧段对应的圆心角度，圆弧半径为：Yr＝Y4-R；

加速段曲线生成公式为：

L6段：匀速段起始时刻为T5，终止时刻为T6＝1.7，Y6＝Y7+(R-R*cosθ)，匀速段曲线生成公式为：Y＝K*T+(Y5-K1*T5)，其中T5≤T≤T6。

L7段：本段瞄准线角度值逐渐减小：起始时刻为T6，终止时刻为T7＝1.8，Y7＝0。

Tr′＝T7，θ＝arctanK1，其中θ为圆弧段对应的圆心角度，圆弧半径为：Yr′＝Y7+R，减速段曲线生成公式为：

步骤7：根据当前时刻T解算位置指令曲线，并将解算得到的与时间T相关的位置指令Y实时传给横滚位置环路的位置命令g_comd_ro_pos；

g_comd_ro_pos＝Y

根据横滚和俯仰旋变实时解算当前瞄准线横滚角g_los_angles.ro；

将位置命令g_comd_ro_pos与当前瞄准线横滚角g_los_angles.ro做差得到位置偏差dbposErr_io＝g_comd_ro_pos-g_los_angles.ro；

对位置偏差dbposErr_io经过PI位置控制器校正后得到校正位置偏差out_pi_ro：

其中gain_ro为横滚通道的比例增益，ω_el是横滚通道积分调节器的控制参数，out_pi_ro是PI调节器横滚输出结果。

将校正位置偏差out_pi_ro经过超前控制器校正得到超前控制器横滚输出结果out_lead_ro：

其中ω_{el_1}，ω_{el_2}为俯仰超前控制器频率参数，out_lead_el是超前控制器横滚输出结果。

将超前控制器横滚输出结果赋值给速度环命令g_ia_cmnd＝out_lead_ro；将速度环命令g_ia_cmnd进行限幅并输出到横滚电机。

步骤8：判断当前瞄准线角度是否大于拍照起始角度g_m_photo_start，如果是，则进入步骤9，否则返回步骤7；

步骤9：执行机构进入动态拍照阶段，给相机发送运动状态连续拍照指令，在匀速行进中进行拍摄；判断当前瞄准线角度是否大于拍照结束角度g_m_photo_stop，如果到达则动态拍照段终止，给相机发送停止拍照指令，否则返回步骤7；

步骤10：判断是否收到工作结束指令，如果是则结束工作流程；否则返回步骤3；

步骤11：设置航道间隔固定角度ΔY为3度，即相机从初始零位每经过3度为两个航道间的一次切换，到达新的航道后开始拍照。相机每经过固定间隔的角度后开始拍照。由于执行机构本身的加速度和速度限制，判断设置航道间隔固定角度是否符合执行机构可实现的角度范围，如果是，则进入步骤12，否则重新设置航道间隔固定角度。

假设执行机构允许最大速度为K_max＝40度/秒，最大加速度为A_max＝800度/秒方，可实现最小角度间隔为设定航道间隔角度

步骤12：生成静态拍照位置指令曲线，由于执行机构本身在运动状态时有速度和加速度的限制，本发明中以系统能正常跟随的最大速度和加速度计算所得的位置指令作为静态拍照位置曲线指令。

在每一次航道切换时采用CLC曲线来实现，以第一个航道切换为例，此航道使执行机构从0位置到3度的位置。分为三段，第一段L1为圆弧段，机构在这个阶段处于加速状态。第二段L2为直线段，执行机构以可达到的较大速度行进。第三段L3为圆弧段，该段使执行机构由较大速度逐渐变为静止。

L1段：由于执行机构本身有速度和加速度限制，因此设计曲线圆弧L1段，需选取系统能跟踪位置指令的最大速度和加速度。在位置-时间曲线中，初始时刻为(T0,Y0),加速阶段终止点为(T1,Y1)，设定加速段从静止加速到速度为K，K为机构所能跟上位置指令的允许速度。加速段圆弧半径为R。圆弧的圆心坐标(Tr,Yr)，Y1为待求解值，T为当前时刻。实例中T1＝0.02，K＝36.5利用以下公式进行求解：

本段瞄准线角度值逐渐增大：

Tr＝T0；θ＝arctanK，其中θ为圆弧段对应的圆心角度。圆弧半径为：圆心纵坐标：Yr＝Y0+R。加速段曲线生成公式为：

L2段：这个阶段执行机构以能达到的最大速度行进，起始点为(T1,Y1)，终止点为(T2,Y2)，实例中T2＝0.1。由于减速阶段也用加速段同样的加速度，因此减速段转动的角度区间和加速段一样，因此有以下等式：

Y3-Y2＝Y1-Y0，Y2＝Y1+ΔY-2*(Y1-Y0)，其中ΔY为航道间隔角度。

L2段起始时刻为T1，终止时刻为T2，

L2段位置曲线角度指令为：Y＝K*T+(Y1-K*T1)，其中T1≤T≤T2

L3段：设计L3段为减速段，和L1段相似，本段使执行机构从具有某一速度状态转为静止状态。减速阶段起点为(T2,Y2)，减速段圆弧半径为R，圆弧的圆心坐标(Tr’,Yr’)，减速段终点坐标为(T3,Y3)，其中R，T3，(Tr’,Yr’)为待求解值，T为当前时刻，实例中T3＝0.15。利用以下公式进行求解：

本段瞄准线角度值逐渐增大：

Tr′＝T3，Y3＝Y0+ΔY，θ＝arctanK，其中θ为圆弧段对应的圆心角度。圆弧半径为：Yr′＝Y3-R，减速段曲线生成公式为：

L4段：本段为静止段，执行机构保持在Y3角度不动，这段时间为等待相机拍照时间，T4＝0.25,Y4＝Y3。

至此执行机构完成第一个航道切换并完成拍照，其余的3度之间的几个航道切换可通过改变初始时刻和初始位置值，用和同样的过程实现。

L5段：最后一个航道L4末端时间T4＝1.5，Y4＝18，T5＝1.6。

本段瞄准线角度值逐渐减小：

Tr＝T4，θ＝arctanK1，其中K1为系统返回时的速度；圆弧半径为，Y5＝Y4-(R-R*cosθ)，其中θ为圆弧段对应的圆心角度，Yr＝Y4-R，加速段曲线生成公式为：

L6段：匀速段起始时刻为T5，终止时刻为T6＝1.9，Y6＝Y7+(R-R*cosθ)，匀速段曲线生成公式为：Y＝K*T+(Y5-K1*T5)，其中T5≤T≤T6。

L7段：本段瞄准线角度值逐渐减小：起始时刻为T6，终止时刻为T7＝2.0，Y7＝0。

Tr′＝T7，θ＝arctanK1，其中θ为圆弧段对应的圆心角度，圆弧半径为：Yr′＝Y7+R，减速段曲线生成公式为：

步骤13：根据当前时刻T解算位置指令曲线，并将解算得到的与时间T相关的位置指令Y实时传递给位置环；位置环输入指令经过数字位置控制器校正，传递到速度环，然后经过数字速度控制器校正后，生成速度命令输出到横滚电机；

步骤14：判断是否达到目标瞄准线角度位置，如果是，则给相机发单次拍照指令，同时开始对拍照计时；否则返回步骤13；判断单次拍照时间是否到，如果时间到，则给照相机发射停止拍照指令；

步骤15：判断是否收到工作结束指令，如果是则结束工作流程，否则，返回步骤3。

图4b为执行机构在两个航道切换时的位置时间图，左边的曲线为位置指令曲线，右边的曲线为执行机构实际响应的位置曲线，该图表明执行机构可以稳定跟踪本发明所设计的位置曲线指令。

通过图4a和4b的对比，现有技术和本发明技术都是在0.15秒内规划执行机构从当前航道跨越3度转到下一个航道。执行机构在响应现有技术生成的位置指令时，在0.255秒左右进入稳定角度。而执行机构在响应本发明设计的位置曲线指令时，在0.215秒已进入稳定角度。因此，在和现有技术同样的指令规划时间下，利用本发明设计规划的位置指令曲线，执行机构在执行该位置指令时，能更快到位进入稳定角度。

通过图4a可以看出，利用现有技术(正弦)所设计的位置曲线指令，执行机构在快到达下一个航道时候，出现了超调。而利用本发明所设计的位置曲线指令，到达下一个航道时没有出现超调，响应曲线较平滑。

通过图5可以看出，从执行机构在0.15秒内即可到达预定的速度，能满足系统提出的在1到12度保持恒定速度的要求。本发明进行位置曲线设计时，兼顾速度切换的平滑，现有技术未从此角度考虑。执行机构在静止和固定速度之间切换时，能快速达到预定的速度值，而且切换过程平稳无超调。

根据实施例所述的位置实例仿真结果可以看出，执行机构在两个位置之间切换或者从某一位置启动达到固定速度时，使用本发明所述的位置指令生成算法，能使执行机构快速平稳无超调地响应，具有较强的工程实用性。

以上具体实施例，是对发明目的，技术方案和有益效果的进一步解释和说明，并不限定本发明保护范围。在运动执行机构位置指令规划时，凡在本发明原则内所做的任何修改，等同替换，改进等，均应包含在本发明的保护范围之内。

Claims (1)

1.一种基于Dubins曲线的稳定平台位置伺服控制方法，其特征在于：包括以下步骤：

步骤1：将起始位置横滚和俯仰瞄准线初始角度值分别对应赋值给横滚和俯仰位置环输入，使伺服机构位置调转，初始化到起始位置；

步骤2：将相机系统垂直于地面的俯仰角度值赋给目标俯仰角，使相机俯仰方向垂直于地面，并保持惯性稳定；

步骤3：获取相机系统横滚方向的工作任务指令，得到当前横滚方向的工作状态；根据所获取的当前横滚方向工作状态，将当前工作任务指令发出给执行机构；执行机构判断是否收到工作任务指令，如果没有收到时，继续等待接收；如果收到指令，则进入步骤4；

步骤4：判断当前工作状态是按照固定速度动态过程拍照，还是到达固定位置静止进行拍摄；如果是按照固定速度动态过程拍照，则进入步骤5，如果是到达固定位置静止进行拍摄，则进入步骤11；

步骤5；执行机构接收行进拍照的速度值g_m_photo_vel和拍摄区间[g_m_photo_start，g_m_photo_stop]；判断接收的速度值是否处于执行机构自身的可实现速度范围内，如果不是，则重新接收速度值，如果是，则进入步骤6；

步骤6：生成动态拍照位置指令曲线，所述动态拍照位置指令曲线为位置-时间曲线，由圆弧段L1、直线段L2和减速段L3组成；圆弧段L1初始点为(T0,Y0)，匀直线段L2起始点为(T1,Y1)，直线段L2的速度为K，圆弧段L1的半径为R₁，圆弧的圆心坐标(Tr,Yr)，T为当前时刻；

当瞄准线角度值逐渐增大时，圆弧段L1的曲线公式为当瞄准线角度值逐渐减小时，圆弧段L1的曲线公式为

直线段L2的公式为Y＝K*T+(Y1-K*T1)；

减速段L3起点为(T2,Y2)，减速段圆弧半径为R₂，减速段圆弧圆心坐标(Tr’,Yr’),减速段终点坐标为(T3,Y3)

当瞄准线角度值逐渐增大时，减速段L3的曲线公式为当瞄准线角度值逐渐减小时，减速段L3的曲线公式为

步骤7：根据当前时刻T解算位置指令曲线，并将解算得到的与时间T相关的位置指令Y实时传给横滚位置环路的位置命令g_comd_ro_pos；根据横滚和俯仰旋变实时解算当前瞄准线横滚角g_los_angles.ro；将位置命令g_comd_ro_pos与当前瞄准线横滚角g_los_angles.ro做差得到位置偏差dbposErr_io＝g_comd_ro_pos-g_los_angles.ro；对位置偏差经过PI位置控制器校正后得到校正位置偏差out_pi_ro；将校正位置偏差out_pi_ro经过超前控制器校正得到超前控制器横滚输出结果out_lead_ro，将超前控制器横滚输出结果赋值给速度环命令g_ia_cmnd＝out_lead_ro；将速度环命令进行限幅并输出到横滚电机；

步骤8：判断当前瞄准线角度是否大于拍照起始角度g_m_photo_start，如果是，则进入步骤9，否则返回步骤7；

步骤9：执行机构进入动态拍照阶段，给相机发送运动状态连续拍照指令，在匀速行进中进行拍摄；判断当前瞄准线角度是否大于拍照结束角度g_m_photo_stop，如果到达则动态拍照段终止，给相机发送停止拍照指令，否则返回步骤7；

步骤10：判断是否收到工作结束指令，如果是则结束工作流程；否则返回步骤3；

步骤11：设置航道间隔固定角度，相机每经过固定间隔的角度后开始拍照；判断设置航道间隔固定角度是否符合执行机构可实现的角度范围，如果是，则进入步骤12，否则重新设置航道间隔固定角度；

步骤12：生成静态拍照位置指令曲线，所述静态拍照位置指令曲线为位置-时间曲线，由圆弧段L1、直线段L2和减速段L3组成；圆弧段L1初始点为(T0,Y0)，匀直线段L2起始点为(T1,Y1)，直线段L2的速度为K，K为执行机构所能跟上位置指令的最大速度，ΔT为选取的T1和T0的间隔时间，AC为执行机构所能跟上的位置指令的最大加速度，圆弧段L1的半径为R₃，圆弧的圆心坐标(Tr,Yr)，T为当前时刻；

当瞄准线角度值逐渐增大时，圆弧段L1的曲线公式为当瞄准线角度值逐渐减小时，圆弧段L1的曲线公式为

直线段L2的公式为Y＝K*T+(Y1-K*T1)；

减速段L3起点为(T2,Y2)，减速段圆弧半径为R₄，减速段圆弧圆心坐标(Tr’,Yr’),减速段终点坐标为(T3,Y3)

当瞄准线角度值逐渐增大时，减速段L3的曲线公式为当瞄准线角度值逐渐减小时，减速段L3的曲线公式为

步骤13：根据当前时刻T解算位置指令曲线，并将解算得到的与时间T相关的位置指令Y实时传递给位置环；位置环输入指令经过数字位置控制器校正，传递到速度环，然后经过数字速度控制器校正后，生成速度命令输出到横滚电机；

步骤14：判断是否达到目标瞄准线角度位置，如果是，则给相机发单次拍照指令，同时开始对拍照计时；否则返回步骤13；判断单次拍照时间是否到，如果时间到，则给照相机发送停止拍照指令；

步骤15：判断是否收到工作结束指令，如果是则结束工作流程，否则，返回步骤3。