CN103399986B - Based on differential geometric space manipulator modeling method - Google Patents

Abstract

The invention discloses a kind of based on differential geometric space manipulator modeling and analysis methods.On the basis of selected mechanical arm system rigid body zero-bit at different levels, calculate and provide the initialization condition such as zero-bit joint vector, barycenter joint vector, inertia matrix, calculate the matrix relevant to inertial parameter and coordinate position matrix.Calculated the generalized velocity of each connecting rod by forward recursive, another mistake is to the generalized force of each connecting rod of recurrence calculation.Finally each amount is substituted into matrix, write as the kinetics equation of compact form.It is unified that this model has model form, and operand is little, improves space manipulator model counting yield and precision, thus improve the design accuracy of space manipulator.The present invention also can be used for the modeling of other Multiple Rigid Body Systems.

Description

Based on differential geometric space manipulator modeling method

Technical field

The invention belongs to space manipulator model construction field, be specifically related to a kind of based on differential geometric space manipulator modeling method.

Background technology

Space manipulator, as spacecraft service equipment in-orbit, can complete the multi-tasks such as spacecraft maintenance, fuel supplement, Material Transportation, space trash cleaning, play an important role in space operation.

Carrying out in the process of conceptual design and study to space manipulator, accurate, simple mathematical model is the basis of research.But, under space manipulator is operated in weightlessness of space environment, between floating pedestal and robot linkage, there is the coupled relation of height.Meanwhile, for the consideration of the financial cost caused during space-ward launching objects, the volume of space manipulator, quality, energy consumption are all very limited.Therefore traditional ground robot modeling method is no longer applicable to space manipulator, needs according to its modeling method of space manipulator the characteristic study.

Find through searching document, for the modeling of space manipulator, current method mainly contains:

(1) Lagrange's equation (EGPapadopoulos.OntheDynamicsandControlofSpaceManipulator s [D] .Dept.Mech.Eng.MIT, Cambridge, 1990.), (2) virtual machine mechanical arm (ZVafa, SDubowsky.TheKinematicsandDynamicsofSpaceManipulators:Th eVirtualManipulatorApproach [J] .Int.JournalofRoboticsResearch, 1990, 9 (4): 3-21.), (3) dynamics arm (Liang Bin of equal value, Liu Liangdong, Li Gengtian. the dynamics equivalent mechanical arm [J] of robot for space. robotization journal .1998, 24 (6): 761-767.), (4) multi rigid body system dynamics method (Chen Li, Liu Yanzhu. the sliding formwork that the Dynamic Modeling of floating foundation Space Manipulator System and inertial track are followed the tracks of controls [J]. mechanics quarterly, 2000, 04:482-486.) etc.The basic thought of above method is all based on classical mechanics, obtains the display kinetics equation of system, gained equation explicit physical meaning, but there is a large amount of differentiating, and causes model calculation amount sharply to increase along with the growth of connecting rod quantity.

Chinese invention patent 200910073470.X, patent name is: robot for space multidomain uniform modeling and analogue system, this patent adopts multi-field physical system modeling language Modelica to develop, pay close attention to the data interaction between each parts of robot, achieve the modeling of crossing over multiple ambit, but gained manipulator model is still classical mechanics model, cannot avoid the defect of Traditional Space mechanical arm modeling method.

The Liu Yunping of Nanjing Aero-Space University (Liu Yunping. the research [D] of spacecraft multi-body system attitude dynamics and control. Nanjing Aero-Space University, 2009.) on the basis of spatial operator algebra theory, binding kinetics equivalent mechanical arm, and by being combined with spinor recurrence calculation by Pseudo velocity, have studied the method for being carried out Dynamic Modeling by Pseudo velocity.The method operation efficiency is high, but still there is differential and ask for and matrix inversion process, can not ensure speed and the precision of real-time operation, and not possess obvious physical significance, be not easy to intuitivism apprehension.

The Shen Hui of the National University of Defense Technology (Shen Hui. the theoretical and control method research [D] of the geometric analysis of parallel robot. the National University of Defense Technology, 2003.) instruments such as Riemannian are utilized to have studied the method for fixed pedestal parallel robot Dynamic Modeling, analyze the Orthogonal Decomposition problem of free velocity subspace on Configuration manifold and constraining force subspace, and adopt the instruments such as riemannian connection to sets forth the geometric model of parallel robot at configuration space and work space.The Huang Xiaohua of Dalian University of Technology (Huang Xiaohua. the Differential Manifold theory and methods [D] of robot manipulation's performance. Dalian University of Technology, 2006.) and Zhang Liandong (Zhang Liandong. based on the research [D] of robot manipulation's performance of differential geometry. Dalian University of Technology, 2004.) etc. people sets forth the Dynamic Models of Robot Manipulators based on infinitesimal geometry method, and applies Riemann curvature and analyze robot dynamics's operating performance.But above-mentioned research is all be applied to ground fixed pedestal mechanical arm system, is not suitable for the space manipulator modeling with floating pedestal characteristic.

Left ancestor's jade of BJ University of Aeronautics & Astronautics (sports immunology of the upper General Mechanics control system of the beautiful .SE (3) of left ancestor and Modeling of Vehicle. aviation journal, 2012, be 08:1491-1497) left constant simple mechanical system based on a class Lagrangian, propose the mathematical definition about SE (3) General Mechanics control system, and give the application example of four rotor unmanned helicopters and unmanned airship modeling.But the method for be that unmanned vehicle, underwater vehicle etc. need to consider potential-energy function change in configuration space, and the system of nonconservative force, different from the condition without potential energy and nonconservative force effect of space manipulator; And acquired results lays particular stress on theory, complex forms, is not suitable for direct computer programming.

Traditional space manipulator modeling method, based on classical mechanics, needs to carry out a large amount of differential and matrix inversion operation, causes that operation time is long, precision is low.The modeling method based on spatial operator algebra improved, can reduce operand, but not possess obvious physical significance, be not easy to intuitivism apprehension.The existing rigid motion modeling based on infinitesimal geometry method, has good performance and physical significance, but is only applicable to ground fixed pedestal mechanical arm, or considers the aircraft of potential energy, is not suitable for space manipulator.The application of current existing infinitesimal geometry in space manipulator, is only confined to the exact linearization method adopting dynamic inversion in the design of controller, does not relate to modeling process.To in the research process of space manipulator, have a kind of demand to be that the model of set up space manipulator can either ensure model counting yield and precision, possess again clear and definite physical significance, current literature search does not also have similar achievement in research and method simultaneously.

Summary of the invention

The object of the invention is to overcome the deficiencies in the prior art part, provide a kind of can improve model counting yield and precision based on differential geometric space manipulator modeling method, thus improve the design accuracy of space manipulator.

The object of the present invention is achieved like this:

A kind of as follows based on differential geometric space manipulator modeling method step:

Step one: selected mechanical arm system zero-bit, writes out zero-bit joint vector S ∈ se (3), barycenter joint vector r, inertia matrix I.

Step 2: calculate the positive definite symmetrical matrix J relevant to inertial parameter, coordinate position matrix M ∈ SE (3), SE (3) represent special Euclidean group.

Step 3: forward recursive calculates the generalized velocity of each connecting rod.

Step 4: the given generalized force F acted on end effector _n+1, backward induction method calculates the generalized force of each connecting rod.

Step 5: each amount is substituted into matrix, is write as the kinetics equation of compact form.

The present invention is based on differential geometric space manipulator modeling method and also have following technical characteristic:

1, being implemented as follows of described step one:

(1) base coordinate system is designated as No. 0, connecting rod at different levels is designated as i (i=1, number n), barycenter and each link joint of selected pedestal connect firmly the initial point of coordinate system respectively as pedestal and connecting rod at different levels, end effector connects firmly coordinate system and is designated as (n+1) number, and initial point is selected in connecting rod end;

(2) each link joint variable q _ibe defined as according to robotics convention

With q _i=0 is zero-bit, wherein θ _irepresent joint rotation angle, d _irepresent joint moving displacement.

2, being implemented as follows of described step 2:

(1) matrix J defined calculates like this

$J_i=\left[\begin{array}{cc}{I}_i-m_i{{\tilde{r}}_i}^2& m_i{\tilde{r}}_i\\ -m_i{\tilde{r}}_i& m_i{E}_{3\×3}\end{array}\right],i=0,\·\·\·,n$

Wherein, m _irepresent the quality of i-th rigid body, E _{3 × 3}represent 3 × 3 unit matrix, r _iit connects firmly the position vector of coordinate system to the barycenter of expression i-th rigid body relatively, represent r _iantisymmetric matrix, namely for r _i=[r _xir _yir _zi] ^t,

${\tilde{r}}_i=\left[\begin{array}{ccc}0& {-r}_{\mathrm{zi}}& r_{\mathrm{yi}}\\ r_{\mathrm{zi}}& 0& {-r}_{\mathrm{xi}}\\ {-r}_{\mathrm{yi}}& r_{\mathrm{xi}}& 0\end{array}\right];$

(2) choosing of coordinate position matrix M ∈ SE (3) is i-th rigid body and connects firmly coordinate system connects firmly coordinate system coordinate transform relative to (i-1) individual rigid body.

3, being implemented as follows of described step 3: recursive process, from i=1 to n, calculates according to following formula

$f_{i-1,i}=M_i{e}^{S_i{q}_i}$

$V_i={\mathrm{Ad}}_{f_{i-1,i}^{-1}}\left(V_{i-1}\right)+S_i{\stackrel{\·}{q}}_i$

${\stackrel{\·}{V}}_i=S_i{\stackrel{\·\·}{q}}_i+{\mathrm{Ad}}_{f_{i-1,i}^{-1}}\left({\stackrel{\·}{V}}_{i-1}\right)-{\mathrm{ad}}_{S_i{\stackrel{\·}{q}}_i}{\mathrm{Ad}}_{f_{i-1,i}^{-1}}\left(V_{i-1}\right)$

$V_0=-\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{m}_i{V}_i/m_0,{\stackrel{\·}{V}}_0=0$

Wherein, f _{i-1, i}represent the exponent product of i-th rigid body relative to (i-1) individual rigid body, V _i, represent generalized velocity and the generalized acceleration of i-th rigid body respectively, computing Ad _x(x), ad _xy () is respectively Lie group, the adjoint of Lie algebra maps.

4, being implemented as follows of described step 4: recursive process, from i=n to 1, calculates according to following formula

$F_i={\mathrm{Ad}}_{f_{i,i+1}^{-1}}^{*}\left(F_{i+1}\right)+J_i{\stackrel{\·}{V}}_i-{\mathrm{ad}}_{V_i}^{*}\left(J_i{V}_i\right)$

${\mathrm{\τ}}_i=S_i^T{F}_i$

Wherein, F _irepresent generalized force suffered by i-th rigid body, τ _irepresent the moment acting on i-th joint, computing ad _x ^*be respectively the adjoint mapping of antithesis of Lie group, Lie algebra.

5, being implemented as follows of described step 5:

Order

V＝[V ₁，V ₂，…，V _n] ^T∈R ^6n×1，F＝[F ₁，F ₂，…，F _n] ^T∈R ^6n×1

$\stackrel{\·}{q}=[{\stackrel{\·}{q}}_1,{\stackrel{\·}{q}}_2,\·\·\·,{\stackrel{\·}{q}}_n{]}^T\∈R^{n\×1},\mathrm{\τ}=[{\mathrm{\τ}}_1,{\mathrm{\τ}}_2,\·\·\·,{\mathrm{\τ}}_n{]}^T\∈R^{n\×1}$

S＝diag[S ₁S ₂…S _n]∈R ^6n×6

J＝diag[J ₁J ₂…J _n]∈R ^6n×6n

${\mathrm{ad}}_{s\stackrel{\·}{q}}=\mathrm{diag}\left[\begin{array}{cccc}{-\mathrm{ad}}_{S_1{\stackrel{\·}{q}}_1}& {-\mathrm{ad}}_{S_2{\stackrel{\·}{q}}_2}& \·\·\·& {-\mathrm{ad}}_{S_n{\stackrel{\·}{q}}_n}\end{array}\right]\∈R^{6n\×6n}$

${\mathrm{ad}}_V^{*}=\mathrm{diag}\left[\begin{array}{cccc}{-\mathrm{ad}}_{V_1}^{*}& {-\mathrm{ad}}_{V_2}^{*}& \·\·\·& {-\mathrm{ad}}_{V_n}^{*}\end{array}\right]\∈R^{6n\×6n}$

Then gained recurrence equation in step 3 and step 4 can be expressed as

$V=\mathrm{\ΓV}+S\stackrel{\·}{q}+P_0{V}_0$

$\stackrel{\·}{V}=\mathrm{\Γ}\stackrel{\·}{V}+S\stackrel{\·\·}{q}+{\mathrm{ad}}_{S\stackrel{\·}{q}}\mathrm{\ΓV}+{\mathrm{ad}}_{S\stackrel{\·}{q}}{P}_0{V}_0+P_0{\stackrel{\·}{V}}_0$

$F={\mathrm{\Γ}}^{T}F+J\stackrel{\·}{V}+{\mathrm{ad}}_V^{*}\mathrm{JV}+P_t^T{F}_{n+1}$

τ＝S ^TF

Further note G=(E-Γ) ^-1∈ R ^{6n × 6n}, namely

Then can obtain Space Manipulator System kinetics equation under free state of flight

$M\left(q\right)\stackrel{\·\·}{q}+C(q,\stackrel{\·}{q})\stackrel{\·}{q}=\mathrm{\τ}$

Wherein M (q)=S ^tg ^tjGS, $C(q,\stackrel{\·}{q})=S^T{G}^T({\mathrm{JGad}}_{S\stackrel{\·}{q}}\mathrm{\Γ}+{\mathrm{ad}}_V^{*}J)\mathrm{GS}.$

The present invention is based on differential geometric space manipulator modeling method, adding with simple matrix the Linear Algebra Operation such as to take advantage of instead of asking for of differential, operation efficiency and precision are improved, the space manipulator model obtained is more accurate, thus improve the design accuracy of space manipulator, meet the requirement of simulation accuracy, simultaneously, there is clear and definite corresponding relation because rigid body describes with Lie Group & Lie Algebra, make gained kinetics equation have the unification of mathematical form and physical significance.The present invention also can be used for the modeling of other Multiple Rigid Body Systems.

Accompanying drawing explanation

Fig. 1 is the present invention for single armed Space Manipulator System universal model schematic diagram;

Fig. 2 is plane 2R free flight Space Manipulator System schematic diagram;

Fig. 3 is the schematic diagram that plane 2R free flight Space Manipulator System connects firmly that coordinate origin is chosen at barycenter place;

Fig. 4 is the open-loop simulation result figure of the plane 2R free flight space manipulator based on infinitesimal geometry method establishment;

Fig. 5 is the closed-loop simulation result figure of the plane 2R free flight space manipulator based on infinitesimal geometry method establishment.

Embodiment

Below in conjunction with accompanying drawing citing, the invention will be further described.

Embodiment 1, composition graphs 1, of the present invention as follows based on differential geometric modeling method step:

Step one: selected mechanical arm system zero-bit, writes out zero-bit joint vector S ∈ se (3), barycenter joint vector r, inertia matrix I.

Step 2: calculate the positive definite symmetrical matrix J relevant to inertial parameter, coordinate position matrix M ∈ SE (3).

Step 3: forward recursive calculates the generalized velocity of each connecting rod.

Step 4: the given generalized force F acted on end effector _n+1, backward induction method calculates the generalized force of each connecting rod.

Step 5: write each amount as expression matrix, obtains the kinetics equation of compact form.

Embodiment 2, composition graphs 1, Fig. 2, the mechanical arm system zero-bit that the present invention studies is chosen like this, and barycenter and each link joint of selected pedestal connect firmly the initial point of coordinate system respectively as pedestal and connecting rod at different levels, base coordinate system is designated as No. 0, connecting rod at different levels be designated as i (i=1 ..., n), end effector connects firmly coordinate system and is designated as (n+1) number, and initial point is selected in connecting rod end.Each link joint variable is defined as according to robotics convention

With q _i=0 is zero-bit.

For plane 2R free flight Space Manipulator System, zero-bit joint vector S _i=[001000] ^t; Centroid position vector r ₀=[000] ^t, r ₁=[a ₁00] ^t, r ₂=[a ₂00] ^t, wherein a ₁, a ₂be respectively diagram geometric parameter; Inertia matrix I ₀=diag [00I _zz0], I ₁=diag [00I _zz1], I ₂=diag [00I _zz2], wherein I _zz0, I _zz1, I _zz2be respectively pedestal and connecting rod at different levels around the moment of inertia connecting firmly coordinate origin separately.

Embodiment 3, composition graphs 1, Fig. 2, the matrix J that the present invention defines and coordinate position matrix M ∈ SE (3) calculate like this,

$J_i=\left[\begin{array}{cc}{I}_i-m_i{{\tilde{r}}_i}^2& m_i{\tilde{r}}_i\\ -m_i{\tilde{r}}_i& m_i{E}_{3\×3}\end{array}\right],i=0,\·\·\·,n$

Wherein, r _iit connects firmly the position vector of coordinate system to the barycenter of expression i-th rigid body relatively, represent r _iantisymmetric matrix, namely for r _i=[r _xir _yir _zi] ^t,

${\tilde{r}}_i=\left[\begin{array}{ccc}0& {-r}_{\mathrm{zi}}& r_{\mathrm{yi}}\\ r_{\mathrm{zi}}& 0& {-r}_{\mathrm{xi}}\\ {-r}_{\mathrm{yi}}& r_{\mathrm{xi}}& 0\end{array}\right]$

A fixed reference system F in given space, then the rotation Θ of a rigid body X=(Θ, b) and translation b can represent with special Euclidean group SE (3), and it is the matrix form of 4 × 4.

$\mathrm{SE}\left(3\right)=\left\{\left[\begin{array}{cc}\mathrm{\Θ}& b\\ 0_{1\×3}& 1\end{array}\right]\right|\mathrm{\Θ}\∈\mathrm{SO}\left(3\right),b\∈R^3\}$

Here SO (3)=and Θ | Θ Θ ^t=E ₃, det (Θ)=+ 1}, represent proper orthogonal group, it by 3 × 3 determinant be 1 orthogonal matrix form.The choosing of coordinate position matrix M ∈ SE (3) is i-th rigid body and connects firmly coordinate system connects firmly coordinate system coordinate transform relative to (i-1) individual rigid body.

For plane 2R free flight Space Manipulator System

$M_1=\left[\begin{array}{cccc}1& 0& 0& b_0\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right],$ $M_2=\left[\begin{array}{cccc}1& 0& 0& l_1\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$

Embodiment 4, composition graphs 1, Fig. 2, the present invention carries out forward recursive to calculate each joint generalized velocity and generalized acceleration.

According to the exponent product method that Brockett proposes, the motion in each joint can be expressed as a group unit of SE (3) or its subgroup, and especially, for rigid body of generally connecting, its conversion can be expressed as index product form.Under the coordinate system being fixed on each connecting rod of mechanical arm, follow the right-hand rule, i-th coordinate system can be expressed as relative to the motion of the i-th-1 coordinate system

$f_{i-1,i}=M_i{e}^{S_i{q}_i},i=\mathrm{1,2},\·\·\·,n$

Wherein M _i∈ SE (3), S _i∈ se (3), q _i∈ R with described in step one and step 2, for there is x ∈ se (3), make exp (x)=X expression according to Rodrigues formulae discovery.

Recursive process, from i=1 to n, calculates according to following formula

$f_{i-1,i}=M_i{e}^{S_i{q}_i}$

$V_i={\mathrm{Ad}}_{f_{i-1,i}^{-1}}\left(V_{i-1}\right)+S_i{\stackrel{\·}{q}}_i$

${\stackrel{\·}{V}}_i=S_i{\stackrel{\·\·}{q}}_i+{\mathrm{Ad}}_{f_{i-1,i}^{-1}}\left({\stackrel{\·}{V}}_{i-1}\right)-{\mathrm{ad}}_{S_i{\stackrel{\·}{q}}_i}{\mathrm{Ad}}_{f_{i-1,i}^{-1}}\left(V_{i-1}\right)$

$V_0=-\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{m}_i{V}_i/m_0,{\stackrel{\·}{V}}_0=0$

Wherein m _irepresent Rigid Mass at different levels, computing Ad _xx adjoint mapping that () is Lie group, to Lie group element X=(Θ, b) ∈ SE (3) and Lie algebra element x ∈ se (3) thereof, calculating formula is

Ad _X(x)＝XxX ^-1

Or

${\mathrm{Ad}}_X\left(x\right)=\left[\begin{array}{cc}\mathrm{\Θ}& 0_{3\×3}\\ \tilde{b}\mathrm{\Θ}& \mathrm{\Θ}\end{array}\right]\left[\begin{array}{c}\mathrm{\ω}\\ v\end{array}\right]\∈R^{6\×6}$

And for arbitrary X, Y ∈ SE (3), have and Ad _xad _y=Ad _xYset up.

Ad _xy () represents that the adjoint of Lie algebra maps, for two element x=(ω in se (3) ₁, v ₁) and y=(ω ₂, v ₂), it is provided by following formula

${\mathrm{ad}}_x\left(y\right)=\left[\begin{array}{cc}{\widetilde{\mathrm{\ω}}}_1& 0_{3\×0}\\ {\tilde{v}}_1& {\widetilde{\mathrm{\ω}}}_1\end{array}\right]\left[\begin{array}{c}{\mathrm{\ω}}_2\\ v_2\end{array}\right]\∈R^{6\×6}$

For plane 2R free flight Space Manipulator System, can calculate

And

${\mathrm{Ad}}_{f_{\mathrm{0,2}}^{-1}}={\left({\mathrm{Ad}}_{f_{\mathrm{0,2}}}\right)}^{-1}={\left({\mathrm{Ad}}_{f_{\mathrm{0,1}}}{\mathrm{Ad}}_{f_{\mathrm{1,2}}}\right)}^{-1}={\mathrm{Ad}}_{f_{\mathrm{1,2}}^{-1}}{\mathrm{Ad}}_{f_{\mathrm{0,1}}^{-1}}$

Embodiment 5, composition graphs 1, Fig. 2, the present invention is at the given generalized force F acted on end effector _n+1condition under, backward induction method calculates the generalized force of each connecting rod.

Recursive process, from i=n to 1, calculates according to following formula

$F_i={\mathrm{Ad}}_{f_{i,i+1}^{-1}}^{*}\left(F_{i+1}\right)+J_i{\stackrel{\·}{V}}_i-{\mathrm{ad}}_{V_i}^{*}\left(J_i{V}_i\right)$

${\mathrm{\τ}}_i=S_i^T{F}_i$

Wherein computing for the antithesis of Lie group is with mapping, its computing operator is Ad _xtransposition.Z=(m, f) ∈ se (3) ^*, then

${\mathrm{Ad}}_X^{*}\left(z\right)=\left[\begin{array}{cc}{\mathrm{\Θ}}^T& {\mathrm{\Θ}}^T{\tilde{b}}^T\\ 0_{3\×3}& {\mathrm{\Θ}}^T\end{array}\right]\left[\begin{array}{c}m\\ f\end{array}\right]\∈R^{6\×6}$

The antithesis of Lie algebra is with ad _x ^*, its operator is ad _xtransposition, be expressed as

${{\mathrm{ad}}_x}^{*}\left(z\right)=\left[\begin{array}{cc}-{\widetilde{\mathrm{\ω}}}_1& -{\tilde{v}}_1\\ 0_{3\×3}& -{\widetilde{\mathrm{\ω}}}_1\end{array}\right]\left[\begin{array}{c}m\\ f\end{array}\right]$

For plane 2R free flight Space Manipulator System, can according to Ad _xobtain.If there is rigid body generalized velocity at different levels

$V_0={\left[\begin{array}{cccccc}0& 0& {\stackrel{\·}{q}}_0& v_{0x}& v_{0y}& 0\end{array}\right]}^T$

$V_1={\left[\begin{array}{cccccc}0& 0& {\stackrel{\·}{q}}_0+{\stackrel{\·}{q}}_1& v_{1x}& v_{1y}& 0\end{array}\right]}^T$

$V_2={\left[\begin{array}{cccccc}0& 0& {\stackrel{\·}{q}}_0+{\stackrel{\·}{q}}_1+{\stackrel{\·}{q}}_2& v_{2x}& v_{2y}& 0\end{array}\right]}^T$

Then can calculate

Embodiment 6, composition graphs 1, Fig. 2, is of the present inventionly write each amount as expression matrix, obtains the kinetics equation of compact form.

Order

V＝[V ₁，V ₂，…，V _n] ^T∈R ^6n×1，F＝[F ₁，F ₂，…，F _n] ^T∈R ^6n×1

$\stackrel{\·}{q}={[{\stackrel{\·}{q}}_1,{\stackrel{\·}{q}}_2,\·\·\·,{\stackrel{\·}{q}}_n]}^T\∈R^{n\×1},\mathrm{\τ}=[{\mathrm{\τ}}_1,{\mathrm{\τ}}_2,\·\·\·,{\mathrm{\τ}}_n{]}^T\∈R^{n\×1}$

S＝diag[S ₁S ₂…S _n]∈R ^6n×6

J＝diag[J ₁J ₂…J _n]∈R ^6n×6n

${\mathrm{ad}}_{s\stackrel{\·}{q}}=\mathrm{diag}\left[\begin{array}{cccc}{-\mathrm{ad}}_{S_1{\stackrel{\·}{q}}_1}& {-\mathrm{ad}}_{S_2{\stackrel{\·}{q}}_2}& \·\·\·& {-\mathrm{ad}}_{S_n{\stackrel{\·}{q}}_n}\end{array}\right]\∈R^{6n\×6n}$

${\mathrm{ad}}_V^{*}=\mathrm{diag}\left[\begin{array}{cccc}{-\mathrm{ad}}_{V_1}^{*}& {-\mathrm{ad}}_{V_2}^{*}& \·\·\·& {-\mathrm{ad}}_{V_n}^{*}\end{array}\right]\∈R^{6n\×6n}$

Then gained recurrence equation in step 3 and step 4 can be expressed as

$V=\mathrm{\ΓV}+S\stackrel{\·}{q}+P_0{V}_0$

$\stackrel{\·}{V}=\mathrm{\Γ}\stackrel{\·}{V}+S\stackrel{\·\·}{q}+{\mathrm{ad}}_{S\stackrel{\·}{q}}\mathrm{\ΓV}+{\mathrm{ad}}_{S\stackrel{\·}{q}}{P}_0{V}_0+P_0{\stackrel{\·}{V}}_0$

$F={\mathrm{\Γ}}^{T}F+J\stackrel{\·}{V}+{\mathrm{ad}}_V^{*}\mathrm{JV}+P_t^T{F}_{n+1}$

τ＝S ^TF

Further note G=(E-Γ) ^-1∈ R ^{6n × 6n}, namely

Then can obtain Space Manipulator System kinetics equation under free state of flight

$M\left(q\right)\stackrel{\·\·}{q}+C(q,\stackrel{\·}{q})\stackrel{\·}{q}=\mathrm{\τ}$

Wherein M (q)=S ^tg ^tjGS, $C(q,\stackrel{\·}{q})=S^T{G}^T({\mathrm{JGad}}_{S\stackrel{\·}{q}}\mathrm{\Γ}+{\mathrm{ad}}_V^{*}J)\mathrm{GS}.$

For plane 2R free flight Space Manipulator System,

V＝[V ₁，V ₂，…，V _n] ^T∈R ^6n×1，F＝[F ₁，F ₂，…，F _n] ^T∈R ^6n×1

$\stackrel{\·}{q}=[{\stackrel{\·}{q}}_1,{\stackrel{\·}{q}}_2,\·\·\·,{\stackrel{\·}{q}}_n{]}^T\∈R^{n\×1},\mathrm{\τ}=[{\mathrm{\τ}}_1,{\mathrm{\τ}}_2,\·\·\·,{\mathrm{\τ}}_n{]}^T\∈R^{n\×1}$

S＝diag[S ₀S ₁S ₂]∈R ^18×3，

J＝diag[J ₀J ₁J ₂]∈R ^18×18

${\mathrm{ad}}_{S\stackrel{\·}{q}}=\mathrm{diag}\left[\begin{array}{ccc}{-\mathrm{ad}}_{S_0{\stackrel{\·}{q}}_0}& {-\mathrm{ad}}_{S_1{\stackrel{\·}{q}}_1}& {-\mathrm{ad}}_{S_2{\stackrel{\·}{q}}_2}\end{array}\right]\∈R^{18\×18}$

${\mathrm{ad}}_V^{*}=\mathrm{diag}\left[\begin{array}{ccc}{-\mathrm{ad}}_{V_0}^{*}& {-\mathrm{ad}}_{V_1}^{*}& {-\mathrm{ad}}_{V_2}^{*}\end{array}\right]\∈R^{18\×18}$

$G=\left[\begin{array}{ccc}{I}_{6\×6}& 0& 0\\ {\mathrm{Ad}}_{f_{\mathrm{0,1}}^{-1}}& I_{6\×6}& 0\\ {\mathrm{Ad}}_{f_{\mathrm{0,2}}^{-1}}& {\mathrm{Ad}}_{f_{\mathrm{1,2}}^{-1}}& I_{6\×6}\end{array}\right]$

Above-mentioned derivation result is substituted into kinetic model expression formula, obtains the plane two connecting rod free flight space manipulator kinetic model of differential equation form

$M\left(q\right)\left[\begin{array}{c}{\stackrel{\·\·}{q}}_0\\ {\stackrel{\·\·}{q}}_1\\ {\stackrel{\·\·}{q}}_2\end{array}\right]+C(q,\stackrel{\·}{q})\left[\begin{array}{c}{\stackrel{\·}{q}}_0\\ {\stackrel{\·}{q}}_1\\ {\stackrel{\·}{q}}_2\end{array}\right]=\left[\begin{array}{c}{\mathrm{\τ}}_0\\ {\mathrm{\τ}}_1\\ {\mathrm{\τ}}_2\end{array}\right]$

Embodiment 7, in composition graphs 1, Fig. 2, Fig. 3 modeling method step one of the present invention, the difference of each rigid body connecting rod local frame of reference is chosen and the intermediary matrix in derivation is changed, but can not have an impact to kinetic model.If such as each local frame of reference initial point is moved to a connecting rod barycenter by joint, then there is invertible matrix

$Q=\mathrm{diag}[{\mathrm{Ad}}_{Q_1},{\mathrm{Ad}}_{Q_2},\·\·\·,{\mathrm{Ad}}_{Q_n}]$

Wherein Q _i=(E ₃,-r _i).Make that above-mentioned each matrix is corresponding to be transformed to

S′＝QS，G′＝QGQ ^-1，J′＝Q ^-TJQ ^-1，Γ′＝QΓQ ^-1

${\mathrm{ad}}_{S\stackrel{\·}{q}}^{\′}={\mathrm{Qad}}_{S\stackrel{\·}{q}}{Q}^{-1},{\mathrm{ad}}_v^{*\′}=Q^{-T}{\mathrm{ad}}_v^{*}{Q}^T$

M′(q)＝S′ ^TG′ ^TJ′G′S′＝S ^TG ^TJGS＝M(q)

$C^{\′}(q,\stackrel{\·}{q})=S^{\′T}{G}^{\′T}(J^{\′}{G}^{\′}{\mathrm{ad}}_{S\stackrel{\·}{q}}{\mathrm{\Γ}}^{\′}+{\mathrm{ad}}_v^{*}{J}^{\′})G^{\′}{S}^{\′}$

$=S^T{G}^T({\mathrm{JGad}}_{S\stackrel{\·}{q}}+{\mathrm{ad}}_v^{*}J)\mathrm{GS}=C(q,\stackrel{\·}{q})$

Embodiment 8, composition graphs 2, Fig. 4, Fig. 5, table 1, the present invention is directed to the emulation of the model that plane 2R free flight space manipulator is set up.For gained model

$M\left(q\right)\left[\begin{array}{c}{\stackrel{\·\·}{q}}_0\\ {\stackrel{\·\·}{q}}_1\\ {\stackrel{\·\·}{q}}_2\end{array}\right]+C(q,\stackrel{\·}{q})\left[\begin{array}{c}{\stackrel{\·}{q}}_0\\ {\stackrel{\·}{q}}_1\\ {\stackrel{\·}{q}}_2\end{array}\right]=\left[\begin{array}{c}{\mathrm{\τ}}_0\\ {\mathrm{\τ}}_1\\ {\mathrm{\τ}}_2\end{array}\right]$

Substitute into the actual parameter of table 1.

First carry out the Matlab emulation of open-loop response, apply the pulse torque [0.1Nm00] of duration 1s when 1s in joint ^t, obtain Fig. 4 simulation result.

Apply PD control law to model, step signal 0.01rad is followed the tracks of, original state x in three joints simultaneously ₀=[000000] ^t, obtain Fig. 5 simulation result.

Table 1 plane 2R free flight Space Manipulator System parameter list

Through checking, the simulation result maximum relative error of the Lagrangian model under gained Open-closed-loop simulation result and identical parameters, the same terms is respectively 0.2% and 0.15%, and classical model obtains extensively accreditation, illustrate that the modeling method based on Lie group is correct effective.

Claims (6)

1., based on a differential geometric space manipulator modeling method, it is characterized in that, step is as follows:

Step one: selected mechanical arm system zero-bit, writes out zero-bit joint vector S ∈ se (3), barycenter joint vector r, inertia matrix I;

Step 2: calculate the positive definite symmetrical matrix J relevant to inertial parameter, coordinate position matrix M ∈ SE (3), SE (3) represent special Euclidean group;

Step 3: forward recursive calculates the generalized velocity of each connecting rod;

Step 4: the given generalized force F acted on end effector _n+1, backward induction method calculates the generalized force of each connecting rod;

Step 5: redefine matrix of variables and in the recursion formula it substitution step 3 and step 4 obtained, write as the kinetics equation of compact form.

2. according to claim 1ly to it is characterized in that based on differential geometric space manipulator modeling method, being implemented as follows of described step one:

(1) base coordinate system is designated as No. 0, connecting rod at different levels is designated as i (i=1, number n), barycenter and each link joint of selected pedestal connect firmly the initial point of coordinate system respectively as pedestal and connecting rod at different levels, end effector connects firmly coordinate system and is designated as (n+1) number, and initial point is selected in connecting rod end;

(2) each link joint variable q _ibe defined as according to robotics convention

With q _i=0 is zero-bit, wherein θ _irepresent joint rotation angle, d _irepresent joint moving displacement.

3. according to claim 1 based on differential geometric space manipulator modeling method, it is characterized in that: being implemented as follows of described step 2:

(1) matrix J defined calculates like this

$J_i=\left[\begin{array}{cc}{I}_i-m_i{\tilde{r}}_i^2& m_i{\tilde{r}}_i\\ -m_i{\tilde{r}}_i& m_i{E}_{3\×3}\end{array}\right],i=0,...,n$

Wherein, m _irepresent the quality of i-th rigid body, E _{3 × 3}represent 3 × 3 unit matrix, r _iit connects firmly the position vector of coordinate system to the barycenter of expression i-th rigid body relatively, represent r _iantisymmetric matrix, namely for r _i=[r _xir _yir _zi] ^t,

${\tilde{r}}_i=\left[\begin{array}{ccc}0& -r_{zi}& r_{yi}\\ r_{zi}& 0& -r_{xi}\\ -r_{yi}& r_{xi}& 0\end{array}\right];$

(2) choosing of coordinate position matrix M ∈ SE (3) is i-th rigid body and connects firmly coordinate system connects firmly coordinate system coordinate transform relative to (i-1) individual rigid body.

4. according to claim 1ly to it is characterized in that based on differential geometric space manipulator modeling method, being implemented as follows of described step 3:

Recursive process, from i=1 to n, calculates according to following formula

$f_{i-1,i}=M_i{e}^{S_i{q}_i}$

$V_i={\mathrm{Ad}}_{f_{i-1,i}^{-1}}\left(V_{i-1}\right)+S_i{\stackrel{\·}{q}}_i$

${\stackrel{\·}{V}}_i=S_i{\stackrel{\·\·}{q}}_i+{\mathrm{Ad}}_{f_{i-1,i}^{-1}}\left({\stackrel{\·}{V}}_{i-1}\right)-{\mathrm{ad}}_{S_i{\stackrel{\·}{q}}_i}{\mathrm{Ad}}_{f_{i-1,i}^{-1}}\left(V_{i-1}\right)$

$V_0=-\underset{i=1}{\overset{n}{\Σ}}{m}_i{V}_i/m_0,{\stackrel{\·}{V}}_0=0$

Wherein, f _{i-1, i}represent the exponent product of i-th rigid body relative to (i-1) individual rigid body, V _i, represent generalized velocity and the generalized acceleration of i-th rigid body respectively, computing Ad _x(x), ad _xy () is respectively Lie group and the adjoint of Lie algebra maps.

5. according to claim 1ly to it is characterized in that based on differential geometric space manipulator modeling method, being implemented as follows of described step 4:

Recursive process, from i=n to 1, calculates according to following formula

$F_i={\mathrm{Ad}}_{f_{i,i+1}^{-1}}^{*}\left(F_{i+1}\right)+J_i{\stackrel{\·}{V}}_i-{\mathrm{ad}}_{V_i}^{*}\left(J_i{V}_i\right)$

${\mathrm{\τ}}_i=S_i^T{F}_i$

Wherein, F _irepresent generalized force suffered by i-th rigid body, τ _irepresent the moment acting on i-th joint, computing be respectively the adjoint mapping of antithesis of Lie group, Lie algebra.

6. according to claim 1ly to it is characterized in that based on differential geometric space manipulator modeling method, being implemented as follows of described step 5:

Redefine following variable:

V＝[V ₁，V ₂，…，V _n] ^T∈R ^6n×1，F＝[F ₁，F ₂，…，F _n] ^T∈R ^6n×1

$\stackrel{\·}{q}={\[{\stackrel{\·}{q}}_1,{\stackrel{\·}{q}}_2,...,{\stackrel{\·}{q}}_n\]}^T\∈R^{n\×1},\mathrm{\τ}={\[{\mathrm{\τ}}_1,{\mathrm{\τ}}_2,...,{\mathrm{\τ}}_n\]}^T\∈R^{n\×1}$

S＝diag[S ₁S ₂…S _n]∈R ^6n×6

J＝diag[J ₁J ₂…J _n]∈R ^6n×6n

${\mathrm{ad}}_{S\stackrel{\·}{q}}=diag\left[\begin{array}{cccc}-{\mathrm{ad}}_{S_1{\stackrel{\·}{q}}_1}& -{\mathrm{ad}}_{S_2{\stackrel{\·}{q}}_2}& ...& -{\mathrm{ad}}_{S_n{\stackrel{\·}{q}}_n}\end{array}\right]\∈R^{6n\×6n}$

${\mathrm{ad}}_V^{*}=diag\left[\begin{array}{cccc}-{\mathrm{ad}}_{V_1}^{*}& -{\mathrm{ad}}_{V_2}^{*}& ...& -{\mathrm{ad}}_{V_n}^{*}\end{array}\right]\∈R^{6n\×6n}$

Then gained recurrence equation in step 3 and step 4 is expressed as

$V=\mathrm{\Γ}V+S\stackrel{\·}{q}+P_0{V}_0$

$\stackrel{\·}{V}=\mathrm{\Γ}\stackrel{\·}{V}+S\stackrel{\·\·}{q}+{\mathrm{ad}}_{S\stackrel{\·}{q}}\mathrm{\Γ}V+{\mathrm{ad}}_{S\stackrel{\·}{q}}{P}_0{V}_0+P_0{\stackrel{\·}{V}}_0$

$F={\mathrm{\Γ}}^{T}F+J\stackrel{\·}{V}+{\mathrm{ad}}_V^{*}JV+P_t^T{F}_{n+1}$

τ＝S ^TF

Further note G=(E-Γ) ^-1∈ R ^{6n × 6n}, namely

Then Space Manipulator System kinetics equation under free state of flight

$M\left(q\right)\stackrel{\·\·}{q}+C(q,\stackrel{\·}{q})\stackrel{\·}{q}=\mathrm{\τ}$

Wherein M (q)=S ^tg ^tjGS, $C(q,\stackrel{\·}{q})=S^T{G}^T({\mathrm{JGad}}_{S\stackrel{\·}{q}}\mathrm{\Γ}+{\mathrm{ad}}_V^{*}J)GS.$