路徑規划 VS 軌跡規划
軌跡規划的目的是將輸入的簡單任務描述變為詳細的運動軌跡描述。注意軌跡和路徑的區別:Trajectory refers to a time history of position, velocity, and acceleration for each degree of freedom. The path provides a pure geometric description of motion.
- Path planning (global)
- The (geometric) path is a sequence of waypoints defining the trajectory coarsely.
- Issues solved at this level: obstacle avoidance, shortest path.
- Trajectory generating (local)
- The path provided by path planning constitutes the input to the trajectory generator.
- Trajectory generator “approximates” the desired path waypoints by a class of polynomial functions and generates a time-based control sequence moving the manipulator/mobile platform from its initial configuration to the destination.
受到驅動機構性能等因素的制約,機器人進行運動軌跡規划時需要考慮加速度約束、速度約束和位置約束等多個約束條件。比如,一般低速運動情況下只要保證關節角度不超限即可,這對運動軌跡規划影響很小。但是當機器人運動速度較快時,關節角加速度和角速度容易超出約束范圍,導致驅動電流過大或者超出限位的事故發生。輕則機器人運動出錯,重則損壞硬件。此時,必須在機器人運動軌跡規划時綜合考慮各種約束條件。
Trajectory = path + timing law
Having the path (way-points), the trajectory is completed by a choice of a timing law. If s(t) = t, the trajectory parametrization is the natural one given by the time.
• Operational (Cartesian) space: p(s) = ( x(s), y(s), z(s) ) ⇒ s = s(t).
• Joint space: q(λ) = (q1(λ), q2(λ), . . . , qn(λ)), where n = DOFs ⇒ λ = λ(t).
The timing law:
• is chosen based on task specifications (stop in a point, move at a constant velocity, etc.);
• may consider optimality criteria (min transfer time, min energy, etc.);
• constraints are imposed by actuator capabilities (e.g. max torque, max velocity) and/or by the task (e.g., max acceleration on payload)
Reflexxes Motion在線軌跡生成庫
Reflexxes Motion Library主要分為三層:接口層提供簡單易用的應用程序接口,隱藏了算法的細節;算法層主要提供在線軌跡生成算法(On-Line Trajectory Generation algorithm);數學運算層提供了最基本的數學運算功能以供算法使用:

提供合理的輸入參數后,可以從輸出端獲取數據,用於底層控制
Input and output values of the on-line trajectory generation algorithm
同步行為(Synchronization Behavior)
Reflexxes的在線軌跡生成算法主要分為以下三步:
Step 1: Calculate the Synchronization Time
Step 2: Synchronization of All Selected DOFs
Step 3: Calculate Output Values
the basic OTG algorithm steps
當涉及多個軸同時運動時Reflexxes中可以設置它們的同步行為,分為無同步、時間同步、相位同步:
- non-synchronized: 多個軸到達各自目標位置的時間不限定,即有的先到有的后到
- time-synchronized: All DOFs, which are selected for trajectory-following control, have to reach their target state of motion Mtrgt at the same time instant, namely at tsync in order to achieve time synchronization
- phase-synchronized: Phase synchronization is the synchronization in position, velocity, acceleration and jerk spaces. It means that, given any instant of time, all variables must complete
the same percentage of their trajectories.
下圖是某3自由度系統(3個軸)分別在無同步、時間同步、相位同步設定下的軌跡曲線。可以看出沒有同步的情況下三個軸到達各自目標點的時間不一致,而時間同步和相位同步下是一致的。
Non-synchronized, time-synchronized, and phase-synchronized trajectories for a system with three degrees of freedom.
To specify the behavior of the Reflexxes Motion Library, the enumeration RMLFlags::SyncBehaviorEnum consists of four elements, and the attribute RMLFlags::SynchronizationBehavior is used to specify the synchronization behavior of the desired trajectory.
- RMLFlags::NO_SYNCHRONIZATION,
- RMLFlags::PHASE_SYNCHRONIZATION_IF_POSSIBLE,
- RMLFlags::ONLY_TIME_SYNCHRONIZATION, and
- RMLFlags::ONLY_PHASE_SYNCHRONIZATION,
在Example 3 — Different Synchronization Behaviors of the Position-based algorithm這個例子中可以修改同步行為,看看具體的差別。軌跡生成器的輸入參數如下圖所示,不同顏色代表了不同的自由度:
程序模擬了偶發事件的產生,傳感器捕獲到事件后Reflexxes可以快速、動態地計算軌跡(react instantaneously to unforeseen sensor events )。程序運行到1000ms時接收到一個傳感器事件,這時將設定一個中間位置(intermediate point / waypoint),軸運動到中間位置后再將目標位置設為最開始的值。比如機器人運行過程中突然遇到障礙物,傳感器檢測到障礙后就可以先改變位置避開障礙,然后再駛向目標。
#include <stdio.h> #include <stdlib.h> #include <fstream> #include <iostream> #include <ReflexxesAPI.h> #include <RMLPositionFlags.h> #include <RMLPositionInputParameters.h> #include <RMLPositionOutputParameters.h> //************************************************************************* // defines #define CYCLE_TIME_IN_SECONDS 0.001 // time step: 1ms #define NUMBER_OF_DOFS 2 int main() { // ******************************************************************** // Variable declarations and definitions bool IntermediateTargetStateSet = false , IntermediateStateReached = false ; int ResultValue = 0 ; double Time = 0.0 ; ReflexxesAPI *RML = NULL ; RMLPositionInputParameters *IP = NULL ; RMLPositionOutputParameters *OP = NULL ; RMLPositionFlags Flags ; // ******************************************************************** // Creating all relevant objects of the Type II Reflexxes Motion Library RML = new ReflexxesAPI( NUMBER_OF_DOFS , CYCLE_TIME_IN_SECONDS ); IP = new RMLPositionInputParameters( NUMBER_OF_DOFS ); OP = new RMLPositionOutputParameters( NUMBER_OF_DOFS ); std::ofstream out("data.txt", std::ios::app); // ******************************************************************** // Set-up the input parameters // In this test program, arbitrary values are chosen. If executed on a // real robot or mechanical system, the position is read and stored in // an RMLPositionInputParameters::CurrentPositionVector vector object. // For the very first motion after starting the controller, velocities // and acceleration are commonly set to zero. The desired target state // of motion and the motion constraints depend on the robot and the // current task/application. // The internal data structures make use of native C data types // (e.g., IP->CurrentPositionVector->VecData is a pointer to // an array of NUMBER_OF_DOFS double values), such that the Reflexxes // Library can be used in a universal way. IP->CurrentPositionVector->VecData [0] = 100.0 ; IP->CurrentPositionVector->VecData [1] = 100.0 ; IP->CurrentVelocityVector->VecData [0] = 0.0 ; IP->CurrentVelocityVector->VecData [1] = 0.0 ; IP->CurrentAccelerationVector->VecData [0] = 0.0 ; IP->CurrentAccelerationVector->VecData [1] = 0.0 ; IP->MaxVelocityVector->VecData [0] = 300.0 ; IP->MaxVelocityVector->VecData [1] = 300.0 ; IP->MaxAccelerationVector->VecData [0] = 400.0 ; IP->MaxAccelerationVector->VecData [1] = 400.0 ; IP->MaxJerkVector->VecData [0] = 500.0 ; IP->MaxJerkVector->VecData [1] = 500.0 ; IP->TargetPositionVector->VecData [0] = 700.0 ; IP->TargetPositionVector->VecData [1] = 300.0 ; IP->TargetVelocityVector->VecData [0] = 0.0 ; IP->TargetVelocityVector->VecData [1] = 0.0 ; // The selection vector contains boolean values to mask single DOFs, for which no output values are calculated. IP->SelectionVector->VecData [0] = true ; IP->SelectionVector->VecData [1] = true ; // ******************************************************************** // Setting the flag for time- and phase-synchronization: // // - RMLPositionFlags::ONLY_TIME_SYNCHRONIZATION for // time-synchronization // - RMLPositionFlags::PHASE_SYNCHRONIZATION_IF_POSSIBLE for // phase-synchronization // // Please feel free to change this flag to see the difference in the // behavior of the algorithm. Flags.SynchronizationBehavior = RMLPositionFlags::ONLY_TIME_SYNCHRONIZATION; // ******************************************************************** // Starting the control loop for(;;) { // Calling the Reflexxes OTG algorithm ResultValue = RML->RMLPosition( *IP , OP , Flags ); if (ResultValue < 0) { printf("An error occurred (%d).\n", ResultValue ); break; } // **************************************************************** // Here, the new state of motion, that is // // - OP->NewPositionVector // - OP->NewVelocityVector // - OP->NewAccelerationVector // // can be used as input values for lower level controllers. In the // most simple case, a position controller in actuator space is // used, but the computed state can be applied to many other // controllers (e.g., Cartesian impedance controllers, // operational space controllers). // **************************************************************** for (int i = 0; i < NUMBER_OF_DOFS; i++) out << OP->NewPositionVector->VecData[i] << ","; out << std::endl; // **************************************************************** // Feed the output values of the current control cycle back to // input values of the next control cycle *IP->CurrentPositionVector = *OP->NewPositionVector ; *IP->CurrentVelocityVector = *OP->NewVelocityVector ; *IP->CurrentAccelerationVector = *OP->NewAccelerationVector ; Time += CYCLE_TIME_IN_SECONDS; // **************************************************************** // In this introductory example, we simple trigger a sensor event // after one second. On a real-world system, trigger signal are // commonly generated based on (unforeseen) sensor signals. This // event changes the input parameters and specifies a // intermediate state of motion, that is, a new desired target // state of motion for the Reflexxes algorithm. if ( ( Time >= 1.0 ) && ( !IntermediateTargetStateSet ) ) { IntermediateTargetStateSet = true; IP->TargetPositionVector->VecData [0] = 550.0 ; IP->TargetPositionVector->VecData [1] = 250.0 ; IP->TargetVelocityVector->VecData [0] = -150.0 ; IP->TargetVelocityVector->VecData [1] = -50.0 ; } // **************************************************************** // After reaching the intermediate state of motion define above // we switch the values of the desired target state of motion // back to the original one. In the documentation and the // description of time- and phase-synchronized motion trajectories, // this switching happens at 3873 milliseconds. if ( ( ResultValue == ReflexxesAPI::RML_FINAL_STATE_REACHED ) && ( !IntermediateStateReached ) ) { IntermediateStateReached = true; IP->TargetPositionVector->VecData [0] = 700.0 ; IP->TargetPositionVector->VecData [1] = 300.0 ; IP->TargetVelocityVector->VecData [0] = 0.0 ; IP->TargetVelocityVector->VecData [1] = 0.0 ; continue; } // **************************************************************** // After the final state of motion is reached, we leave the loop // and terminate the program. if (ResultValue == ReflexxesAPI::RML_FINAL_STATE_REACHED) { break; } } // ******************************************************************** // Deleting the objects of the Reflexxes Motion Library end terminating // the process delete RML ; delete IP ; delete OP ; exit(EXIT_SUCCESS) ; }
從上面的軌跡曲線圖中很難看出時間同步和相位同步有什么差別,但是將兩個自由度的位置畫成散點圖就可以發現差異:
可以看出時間同步只是兩個軸會同時到達各自目標位置,而相位同步在此基礎上還限定了其相位關系(在信號處理中如果兩個信號的頻率相等,相位差為0或一個常數,稱這兩個信號相位同步)。設置成相位同步后兩個自由度的位置軌跡呈線性關系,在圖中表現為一條從起點到目標點的直線:p2(t) = ( p1(t) - 100 ) / 3 + 100
參考:
V-rep學習筆記:Reflexxes Motion Library 1
V-rep學習筆記:Reflexxes Motion Library 4
周期同步位置模式(CSP),輪廓位置模式(PPM),位置模式(PM)
On-Line Trajectory Generation in Robotic Systems
Introduction to Robotics-Mechanics and Control. Chapter 7 Trajectory generation