CN104111071B - High-precision position posture calculating method based on laser ranging and camera visual fusion - Google Patents

Abstract

The invention discloses a high-precision position posture calculating method based on the laser ranging and the camera visual fusion. The high-precision position posture calculating method comprises the steps: (S1) performing linear modeling based on a camera visual position posture measuring algorithm of an NLSM (Non-linear Sigma Model) of a cooperative target; (S2) performing the linear modeling based on a multipoint laser range finder; and (S3) performing the linear modeling based on the laser ranging and the camera fusion. High-precision laser ranging information and high-precision angular resolution information of the camera are comprehensively utilized, so that the measuring precision of the position posture of the target is effectively improved, and the problem of fusion of the cooperative target-based laser ranging and the camera visual position posture measuring algorithm is solved.

Description

The high precision position and posture calculation method being merged with camera vision based on laser ranging

Technical field

The present invention relates to test field of measuring technique, merged based on laser ranging and camera vision particularly to a kind of High precision position and posture calculation method.

Background technology

For sar (synthetic aperture radar, synthetic aperture radar) imaging satellite, the deformation pair of antenna The imaging resolution of satellite, picture quality have great impact.Spaceborne sar antenna can be subject to space each during in orbit Plant the effect of load, these load mainly include thermal force, the Gradient of Gravitation square, atmospheric drag and satellite motion interference etc., from And lead to aerial panel to vibrate, reduce the attitude stability of satellite and pointing accuracy, and have a strong impact on the performance of antenna, lead to into As fuzzy, resolution reduce；More seriously vibration may lead to antenna structure to destroy.

Therefore, after the in-orbit expansion of sar satellite large-scale antenna, need to carry out in-orbit real-time high-precision measurement by measuring system Obtaining the change of shape of antenna, to control it when deformation is larger, thus meeting use requirement, and currently lacking Can effectively in real time high-precision determine target antenna relative position and attitude scheme.

During pose resolves, it is commonly applied to cooperative target, so-called cooperative target is exactly set n on measured target (generally at least 3) control point known to position, and video camera known to a position is set on following the tracks of object, due to cooperation As long as the position of target and video camera, it is known that measuring the relative position between cooperative target and video camera and attitude, just can be obtained Relative position between measured target and tracking object and attitude.And this method resolves complicated, exist again and solve phenomenons, for a long time more Since research focus primarily upon and how to solve and in the pure mathematics problem of how many solution lower that explains the situation, therefore, for reality The research tool of border engineer applied is of great significance.

Content of the invention

The present invention is directed to deficiencies of the prior art, there is provided one kind is melted with camera vision based on laser ranging The high precision position and posture calculation method closing, the present invention is achieved through the following technical solutions:

A kind of high precision position and posture calculation method being merged with camera vision based on laser ranging, is comprised the following steps:

S1, carry out the camera vision position-pose measurement linear modelling of nlsm based on cooperative target, the one-tenth of video camera As model:

$\left\{\begin{array}{c}{u}_i=f\frac{z_{\mathrm{ci}}}{x_{\mathrm{ci}}}\\ v_i=f\frac{y_{\mathrm{ci}}}{x_{\mathrm{ci}}}\end{array}\right.i=\mathrm{1,2,3}$

Wherein, (u_i, v_i) it is i-th cooperative target in camera image plane coordinate system (o_f-u_fv_f) in coordinate, (x_ci, y_ci,z_ci) it is corresponding cooperative target in image space coordinate system (o_c-x_cy_cz_c) in coordinate, f be focal length of camera.Image plane is sat Relation between mark system, image space coordinate system coordinate and target-based coordinate system is as shown in Figure 2.Image space coordinate system and target-based coordinate system Coordinate Conversion be:

$\left[\begin{array}{c}{x}_{\mathrm{ci}}\\ y_{\mathrm{ci}}\\ z_{\mathrm{ci}}\end{array}\right]=m_{\mathrm{ct}}\left[\begin{array}{c}{x}_{\mathrm{ti}}\\ x_{\mathrm{ti}}\\ x_{\mathrm{ti}}\end{array}\right]+\left[\begin{array}{c}{t}_{\mathrm{xc}}\\ t_{\mathrm{yc}}\\ t_{\mathrm{zc}}\end{array}\right],i=\mathrm{1,2,3}$

Wherein, (x_ti, y_ti, z_ti) it is coordinate in target-based coordinate system for the cooperative target, (t_xc, t_yc, t_zc) it is coordinates of targets It is coordinate in image space coordinate system for the initial point, m_ctIt is tied to the transition matrix of image space coordinate system, (θ for coordinates of targets₁,θ₂,θ₃) For the corresponding anglec of rotation, m_ctIt is expressed as:

$m_{\mathrm{ct}}=\left[\begin{array}{ccc}\cos {\mathrm{\θ}}_2\cos {\mathrm{\θ}}_3-\sin {\mathrm{\θ}}_1\sin {\mathrm{\θ}}_2\sin {\mathrm{\θ}}_3& \cos {\mathrm{\θ}}_2\sin {\mathrm{\θ}}_3+\sin {\mathrm{\θ}}_1\sin {\mathrm{\θ}}_2\cos {{\mathrm{\θ}}_{}}_3& -\cos {\mathrm{\θ}}_1\sin {\mathrm{\θ}}_2\\ -\cos {\mathrm{\θ}}_1\sin {\mathrm{\θ}}_3& \cos {\mathrm{\θ}}_1\cos {\mathrm{\θ}}_3& \sin {\mathrm{\θ}}_1\\ \sin {\mathrm{\θ}}_2\cos {\mathrm{\θ}}_3+\cos {\mathrm{\θ}}_2\sin {\mathrm{\θ}}_1\sin {\mathrm{\θ}}_3& \sin {\mathrm{\θ}}_2\sin {\mathrm{\θ}}_3-\sin {\mathrm{\θ}}_1\cos {\mathrm{\θ}}_2\cos {\mathrm{\θ}}_3& \cos {\mathrm{\θ}}_1\cos {\mathrm{\θ}}_2\end{array}\right]$

Order

$\left\{\begin{array}{c}{f}_{2i-1}\left(\mathrm{\χ}\right)=f\frac{z_{\mathrm{ci}}}{x_{\mathrm{ci}}}\\ f_{2i}\left(\mathrm{\χ}\right)=f\frac{y_{\mathrm{ci}}}{x_{\mathrm{ci}}}\end{array}\right.i=\mathrm{1,2,3}$

Wherein, χ=(t_xc,t_yc,t_zc,θ₁,θ₂,θ₃), according to nlsm principle, obtain:

min(bε-l)^t(bε-l)

Wherein, χ⁰Initial estimate for χ, ε is χ⁰Corrected value, symbol " t " representing matrix transposition, matrix b be function f (χ) First-order partial derivative, be expressed as:

$b=\frac{\&partiald;f}{\&partiald;\mathrm{\χ}}{|}_{{\mathrm{\χ}}^0}$

Element l in matrix l_iIt is expressed as:

$\left\{\begin{array}{c}{l}_{2i-1}=u_i-f_{2i-1}\left({\mathrm{\χ}}^0\right)\\ l_{2i}=v_i-f_{2i}\left({\mathrm{\χ}}^0\right)\end{array}\right.i=\mathrm{1,2,3}$

S2, the linear modelling carrying out based on multiple spot laser range finder:

Coordinate in image space coordinate system for the installation site p point of laser range finder is (x_pc, y_pc, z_pc), p and cooperative target Laser ranging distance between mark is d_i, i=1,2,3；

$d_i\left(\mathrm{\χ}\right)={(x_{\mathrm{cp}}-x_{\mathrm{ci}})}^2+{(y_{\mathrm{cp}}-y_{\mathrm{ci}})}^2+{(z_{\mathrm{cp}}-z_{\mathrm{ci}})}^2-d_i^2,i=\mathrm{1,2,3}$

Linearisation is carried out to it, obtains:

$d_i\left(\mathrm{\χ}\right)=d_i\left({\mathrm{\χ}}^0\right)+\frac{\&partiald;d_i}{\&partiald;\mathrm{\χ}}\mathrm{\ϵ},i=\mathrm{1,2,3}$

It is rewritten as matrix form, then have:

D (χ)=d (χ⁰)+cε

Wherein, c is the first-order partial derivative of conditional equation d (χ), is:

$c=\frac{\&partiald;d}{\&partiald;\mathrm{\χ}}{|}_{{\mathrm{\χ}}^0}$

S3, the fusion linear modelling carrying out based on laser ranging and video camera:

Using laser ranging as the mandatory constraints of camera vision Measurement Algorithm, then have:

d(χ⁰)+c ε=0

Corresponding object function is:

$\min \left[{(\underset{\mathrm{6,6}}{b}\underset{\mathrm{6,1}}{\mathrm{\ϵ}}-\underset{\mathrm{6,1}}{l})}^t\right(\underset{\mathrm{6,6}}{b}\underset{\mathrm{6,1}}{\mathrm{\ϵ}}-\underset{\mathrm{6,1}}{l})+2{(\underset{\mathrm{3,1}}{d}+\underset{\mathrm{3,6}}{c}\underset{\mathrm{6,1}}{\mathrm{\ϵ}})}^t\underset{\mathrm{3,1}}{\mathrm{\λ}}]$

In formula, λ is the contact number vector limiting conditional equation, and solution formula is expressed as:

λ=[c (b^tb)^-1c^t]^-1[d+c(b^tb)^-1(b^tl)]

Corresponding ε is solved:

ε=(b^tb)^-1(b^tl-c^tλ)

Corresponding position and attitude parameter χ is expressed as:

χ^(k+1)=χ^(k)+ε

In formula, χ^(k)Result of calculation for kth time.

The invention comprehensively utilizes the high-precision ranging information of laser ranging and video camera high-precision angular resolution information, Effectively increase the position and attitude certainty of measurement of target, and solve laser ranging and the video camera having merged based on cooperative target The problem of the position and attitude Measurement Algorithm of vision.

Brief description

Shown in Fig. 1 is the flow chart of the present invention.

Shown in Fig. 2 is the relation between camera plane coordinate system, image space coordinate system and target-based coordinate system.

Specific embodiment

Below with reference to the accompanying drawing of the present invention, clear, complete description is carried out to the technical scheme in the embodiment of the present invention With discussion it is clear that a part of example of the only present invention as described herein, it is not whole examples, based on the present invention In embodiment, the every other enforcement that those of ordinary skill in the art are obtained on the premise of not making creative work Example, broadly falls into protection scope of the present invention.

For the ease of the understanding to the embodiment of the present invention, make further below in conjunction with accompanying drawing taking specific embodiment as a example Illustrate, and each embodiment does not constitute the restriction to the embodiment of the present invention.

As shown in figure 1, a kind of high precision position and posture solution being merged with camera vision based on laser ranging disclosed by the invention Calculation method, comprises the following steps:

S1, carry out the camera vision position-pose measurement linear modelling of nlsm based on cooperative target, the one-tenth of video camera As model:

$\left\{\begin{array}{c}{u}_i=f\frac{z_{\mathrm{ci}}}{x_{\mathrm{ci}}}\\ v_i=f\frac{y_{\mathrm{ci}}}{x_{\mathrm{ci}}}\end{array}\right.i=\mathrm{1,2,3}$

Wherein, (u_i, v_i) it is i-th cooperative target in camera image plane coordinate system (o_f-u_fv_f) in coordinate, (x_ci, y_ci,z_ci) it is corresponding cooperative target in image space coordinate system (o_c-x_cy_cz_c) in coordinate, f be focal length of camera.Image plane is sat Relation between mark system, image space coordinate system coordinate and target-based coordinate system is as shown in Figure 2.Image space coordinate system and target-based coordinate system Coordinate Conversion be:

$\left[\begin{array}{c}{x}_{\mathrm{ci}}\\ y_{\mathrm{ci}}\\ z_{\mathrm{ci}}\end{array}\right]=m_{\mathrm{ct}}\left[\begin{array}{c}{x}_{\mathrm{ti}}\\ x_{\mathrm{ti}}\\ x_{\mathrm{ti}}\end{array}\right]+\left[\begin{array}{c}{t}_{\mathrm{xc}}\\ t_{\mathrm{yc}}\\ t_{\mathrm{zc}}\end{array}\right],i=\mathrm{1,2,3}$

Wherein, (x_ti, y_ti, z_ti) it is coordinate in target-based coordinate system for the cooperative target, (t_xc, t_yc, t_zc) it is coordinates of targets It is coordinate in image space coordinate system for the initial point, m_ctIt is tied to the transition matrix of image space coordinate system, (θ for coordinates of targets₁,θ₂,θ₃) For the corresponding anglec of rotation, m_ctIt is expressed as:

$m_{\mathrm{ct}}=\left[\begin{array}{ccc}\cos {\mathrm{\θ}}_2\cos {\mathrm{\θ}}_3-\sin {\mathrm{\θ}}_1\sin {\mathrm{\θ}}_2\sin {\mathrm{\θ}}_3& \cos {\mathrm{\θ}}_2\sin {\mathrm{\θ}}_3+\sin {\mathrm{\θ}}_1\sin {\mathrm{\θ}}_2\cos {{\mathrm{\θ}}_{}}_3& -\cos {\mathrm{\θ}}_1\sin {\mathrm{\θ}}_2\\ -\cos {\mathrm{\θ}}_1\sin {\mathrm{\θ}}_3& \cos {\mathrm{\θ}}_1\cos {\mathrm{\θ}}_3& \sin {\mathrm{\θ}}_1\\ \sin {\mathrm{\θ}}_2\cos {\mathrm{\θ}}_3+\cos {\mathrm{\θ}}_2\sin {\mathrm{\θ}}_1\sin {\mathrm{\θ}}_3& \sin {\mathrm{\θ}}_2\sin {\mathrm{\θ}}_3-\sin {\mathrm{\θ}}_1\cos {\mathrm{\θ}}_2\cos {\mathrm{\θ}}_3& \cos {\mathrm{\θ}}_1\cos {\mathrm{\θ}}_2\end{array}\right]$

Order

$\left\{\begin{array}{c}{f}_{2i-1}\left(\mathrm{\χ}\right)=f\frac{z_{\mathrm{ci}}}{x_{\mathrm{ci}}}\\ f_{2i}\left(\mathrm{\χ}\right)=f\frac{y_{\mathrm{ci}}}{x_{\mathrm{ci}}}\end{array}\right.i=\mathrm{1,2,3}$

Wherein, χ=(t_xc,t_yc,t_zc,θ₁,θ₂,θ₃), according to nlsm principle, obtain:

min(bε-l)^t(bε-l)

Wherein, χ⁰Initial estimate for χ, ε is χ⁰Corrected value, symbol " t " representing matrix transposition.Matrix b is function f (χ) First-order partial derivative, be expressed as:

$b=\frac{\&partiald;f}{\&partiald;\mathrm{\χ}}{|}_{{\mathrm{\χ}}^0}$

Element l in matrix l_iIt is expressed as:

$\left\{\begin{array}{c}{l}_{2i-1}=u_i-f_{2i-1}\left({\mathrm{\χ}}^0\right)\\ l_{2i}=v_i-f_{2i}\left({\mathrm{\χ}}^0\right)\end{array}\right.i=\mathrm{1,2,3}$

S2, the linear modelling carrying out based on multiple spot laser range finder:

Coordinate in image space coordinate system for the installation site p point of laser range finder is (x_pc, y_pc, z_pc), p and cooperative target Laser ranging distance between mark is d_i, i=1,2,3；

$d_i\left(\mathrm{\χ}\right)={(x_{\mathrm{cp}}-x_{\mathrm{ci}})}^2+{(y_{\mathrm{cp}}-y_{\mathrm{ci}})}^2+{(z_{\mathrm{cp}}-z_{\mathrm{ci}})}^2-d_i^2,i=\mathrm{1,2,3}$

Linearisation is carried out to it, obtains:

$d_i\left(\mathrm{\χ}\right)=d_i\left({\mathrm{\χ}}^0\right)+\frac{\&partiald;d_i}{\&partiald;\mathrm{\χ}}\mathrm{\ϵ},i=\mathrm{1,2,3}$

It is rewritten as matrix form, then have:

D (χ)=d (χ⁰)+cε

Wherein, c is the first-order partial derivative of conditional equation d (χ), is:

$c=\frac{\&partiald;d}{\&partiald;\mathrm{\χ}}{|}_{{\mathrm{\χ}}^0}$

S3, the fusion linear modelling based on laser ranging and video camera:

Using laser ranging as the mandatory constraints of camera vision Measurement Algorithm, then have:

d(χ⁰)+c ε=0

Corresponding object function is:

$\min \left[{(\underset{\mathrm{6,6}}{b}\underset{\mathrm{6,1}}{\mathrm{\ϵ}}-\underset{\mathrm{6,1}}{l})}^t\right(\underset{\mathrm{6,6}}{b}\underset{\mathrm{6,1}}{\mathrm{\ϵ}}-\underset{\mathrm{6,1}}{l})+2{(\underset{\mathrm{3,1}}{d}+\underset{\mathrm{3,6}}{c}\underset{\mathrm{6,1}}{\mathrm{\ϵ}})}^t\underset{\mathrm{3,1}}{\mathrm{\λ}}]$

Corresponding λ is solved:

λ=[c (b^tb)^-1c^t]^-1[d+c(b^tb)^-1(b^tl)]

Corresponding ε is solved:

ε=(b^tb)^-1(b^tl-c^tλ)

Corresponding position and attitude parameter χ is expressed as:

χ^(k+1)=χ^(k)+ε

In formula, χ^(k)Result of calculation for kth time.

Below by way of numerical simulation to the laser ranging based on three cooperative target punctuates for the present embodiment and camera vision phase In conjunction with high precision position and posture calculation method verified:

If relevant parameter is as follows:

1st, camera parameter:

Focal length: 0.105m

Camera target surface: 4400 × 6600

Pixel Dimensions: 5.5e-6m

Pixel extraction precision: 0.1pixel

2nd, laser ranging instrument parameter:

Installation site: [0.1264,0.2445,0.4174] m

Range accuracy: better than 0.1mm

3rd, experimental data

Table 1 gives the actual position of index point in each bin, and table 2 gives corresponding measurement error.Table 3 gives by mistake The result of difference statistics.

Table 1

Table 2

	x(mm)	y(mm)	z(mm)	Bin angle (°)
					Max value of error	0.189	0.287	1.140	0.035
Error mean	0.078	0.124	0.356	0.0127

Table 3

Error statistics in table 3, max value of error is respectively as follows: with error mean statistical formula

$x\max =\underset{i=\mathrm{1,2}...n}{\max }\left(\right|x_{\mathrm{ri}}-x_{\mathrm{ci}}\left|\right),\stackrel{\→}{x}=\frac{\underset{i=1}{\overset{n}{\mathrm{\σ}}}|x_{\mathrm{ri}}-x_{\mathrm{ci}}|}{n}$

X in formula_riFor actual value, x_ciFor measured value.

Table 2 realizes 50 meters about of distance with as shown by data in table 3, the maximum pact of the measurement error based on three cooperative targets In 1.14mm, 0.0127 ° about of angular surveying maximum error.

The above, the only present invention preferably specific embodiment, but protection scope of the present invention is not limited thereto, Any those familiar with the art the invention discloses technical scope in, the change or replacement that can readily occur in, All should be included within the scope of the present invention.Therefore, protection scope of the present invention should be with scope of the claims It is defined.

Claims (1)

1. a kind of high precision position and posture calculation method being merged based on laser ranging and camera vision it is characterised in that include with Lower step:

S1, carry out the camera vision position-pose measurement linear modelling of nlsm based on cooperative target, the imaging mould of video camera Type:

$\left\{\begin{array}{c}{u}_i=f\frac{z_{\mathrm{ci}}}{x_{\mathrm{ci}}}\\ v_i=f\frac{y_{\mathrm{ci}}}{x_{\mathrm{ci}}}\end{array}\right.i=\mathrm{1,2,3}$

Wherein, (u_i, v_i) it is i-th cooperative target coordinate in camera plane coordinate system, (x_ci, y_ci, z_ci) it is corresponding Coordinate in image space coordinate system for the cooperative target, image space coordinate system with the Coordinate Conversion of target-based coordinate system is:

$\left[\begin{array}{c}{x}_{\mathrm{ci}}\\ y_{\mathrm{ci}}\\ z_{\mathrm{ci}}\end{array}\right]=m_{\mathrm{ct}}\left[\begin{array}{c}{x}_{\mathrm{ti}}\\ x_{\mathrm{ti}}\\ x_{\mathrm{ti}}\end{array}\right]+\left[\begin{array}{c}{t}_{\mathrm{xc}}\\ t_{\mathrm{yc}}\\ t_{\mathrm{zc}}\end{array}\right],i=\mathrm{1,2,3}$

Wherein, (x_ti, y_ti, z_ti) it is coordinate in target-based coordinate system for the cooperative target, (t_xc, t_yc, t_zc) former for target-based coordinate system Coordinate in image space coordinate system for the point, m_ctIt is tied to the transition matrix of image space coordinate system, (θ for coordinates of targets₁,θ₂,θ₃) it is phase The anglec of rotation answered, m_ctIt is expressed as:

$m_{\mathrm{ct}}=\left[\begin{array}{ccc}\cos {\mathrm{\θ}}_2\cos {\mathrm{\θ}}_3-\sin {\mathrm{\θ}}_1\sin {\mathrm{\θ}}_2\sin {\mathrm{\θ}}_3& \cos {\mathrm{\θ}}_2\sin {\mathrm{\θ}}_3+\sin {\mathrm{\θ}}_1\sin {\mathrm{\θ}}_2\cos {{\mathrm{\θ}}_{}}_3& -\cos {\mathrm{\θ}}_1\sin {\mathrm{\θ}}_2\\ -\cos {\mathrm{\θ}}_1\sin {\mathrm{\θ}}_3& \cos {\mathrm{\θ}}_1\cos {\mathrm{\θ}}_3& \sin {\mathrm{\θ}}_1\\ \sin {\mathrm{\θ}}_2\cos {\mathrm{\θ}}_3+\cos {\mathrm{\θ}}_2\sin {\mathrm{\θ}}_1\sin {\mathrm{\θ}}_3& \sin {\mathrm{\θ}}_2\sin {\mathrm{\θ}}_3-\sin {\mathrm{\θ}}_1\cos {\mathrm{\θ}}_2\cos {\mathrm{\θ}}_3& \cos {\mathrm{\θ}}_1\cos {\mathrm{\θ}}_2\end{array}\right]$

Order

$\left\{\begin{array}{c}{f}_{2i-1}\left(\mathrm{\χ}\right)=f\frac{z_{\mathrm{ci}}}{x_{\mathrm{ci}}}\\ f_{2i}\left(\mathrm{\χ}\right)=f\frac{y_{\mathrm{ci}}}{x_{\mathrm{ci}}}\end{array}\right.i=\mathrm{1,2,3}$

Wherein, χ=(t_xc,t_yc,t_zc,θ₁,θ₂,θ₃), according to nlsm principle, obtain:

minv^tV=min (b ε-l)^t(bε-l)

Wherein, χ⁰Initial estimate for χ, ε is χ⁰Corrected value, symbol " t " representing matrix transposition, matrix b be function f (χ) one Rank partial derivative, is expressed as:

$b=\frac{\&partiald;f}{\&partiald;\mathrm{\χ}}{|}_{{\mathrm{\χ}}^0}$

Element l in matrix l_iIt is expressed as:

$\left\{\begin{array}{c}{l}_{2i-1}=u_i-f_{2i-1}\left({\mathrm{\χ}}^0\right)\\ l_{2i}=v_i-f_{2i}\left({\mathrm{\χ}}^0\right)\end{array}\right.i=\mathrm{1,2,3}$

S2, the linear modelling carrying out based on multiple spot laser range finder:

Coordinate in image space coordinate system for the installation site p point of laser range finder is (x_pc, y_pc, z_pc), p and cooperative target it Between laser ranging distance be d_i, i=1,2,3；

$d_i\left(\mathrm{\χ}\right)={(x_{\mathrm{cp}}-x_{\mathrm{ci}})}^2+{(y_{\mathrm{cp}}-y_{\mathrm{ci}})}^2+{(z_{\mathrm{cp}}-z_{\mathrm{ci}})}^2-d_i^2,i=\mathrm{1,2,3}$

Linearisation is carried out to it, obtains:

$d_i\left(\mathrm{\χ}\right)=d_i\left({\mathrm{\χ}}^0\right)+\frac{\&partiald;d_i}{\&partiald;\mathrm{\χ}}\mathrm{\ϵ},i=\mathrm{1,2,3}$

It is rewritten as matrix form, then have:

D (χ)=d (χ⁰)+cε

Wherein, c is the first-order partial derivative of conditional equation d (χ), is:

$c=\frac{\&partiald;d}{\&partiald;\mathrm{\χ}}{|}_{{\mathrm{\χ}}^0}$

S3, the fusion linear modelling carrying out based on laser ranging and video camera:

Using laser ranging as the mandatory constraints of camera vision Measurement Algorithm, then have:

d(χ⁰)+c ε=0

Corresponding object function is:

$\min \left[{(\underset{\mathrm{6,6}}{b}\underset{\mathrm{6,1}}{\mathrm{\ϵ}}-\underset{\mathrm{6,1}}{l})}^t\right(\underset{\mathrm{6,6}}{b}\underset{\mathrm{6,1}}{\mathrm{\ϵ}}-\underset{\mathrm{6,1}}{l})+2{(\underset{\mathrm{3,1}}{d}+\underset{\mathrm{3,6}}{c}\underset{\mathrm{6,1}}{\mathrm{\ϵ}})}^t\underset{\mathrm{3,1}}{\mathrm{\λ}}]$

Corresponding λ is solved:

λ=[c (b^tb)^-1c^t]^-1[d+c(b^tb)^-1(b^tl)]

Corresponding ε is solved:

ε=(b^tb)^-1(b^tl-c^tλ)

Corresponding position and attitude parameter χ is expressed as:

χ^(k+1)=χ^(k)+ε

In formula, χ^(k)Result of calculation for kth time.